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ABSTRACT 

Control  of  the  motions  and  vibrations  of  large  space  structures  require  the  know- 
ledge of  state  values  that  may  not  be  available  due  either  to  inability  to  measure  the 
states  or,  the  high  cost  of  the  sensors  to  measure  the  required  states.  One  solution  is  the 
use  of  an  observer  to  estimate  the  states  from  limited  sensor  input. 

The  physical  characteristics  of  large  space  structures  and  the  enviroment  they  oper- 
ate in  will  cause  large  amounts  of  noise  in  the  measurements.  The  obvious  observer  for 
such  an  enviroment  is  the  Kalman  Filter  which  is  specifically  designed  to  produce  opti- 
mal estimates  in  a  noisy  enviroment. 

A  straightforward  application  of  the  Kalman  Filter  will  be  examined  utilizing  a 
steady  state  Kalman  gain  matrix.  The  observer  performance  will  be  examined  in  both 
matched  filter,  plant  and  reduced  order  filter  configurations. 
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THESIS  DISCLAIMER 

The  reader  is  cautioned  that  computer  programs  developed  in  this  research  may  not 
have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been  made,  within 
the  time  available,  to  ensure  that  the  programs  are  free  of  computational  and  logic  er- 
rors, they  cannot  be  considered  validated.  Any  application  of  these  programs  without 
additional  verification  is  at  the  risk  of  the  user. 
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I.     INTRODUCTION 

A.  BACKGROUND 

The  advent  of  large  space  structures  poses  a  number  of  problems  for  the  control 
engineer.  Previously,  the  objects  put  into  space  could  be  treated  as  rigid  bodies  so  that 
a  single  three  axis  sensor  package  could  be  used  to  tell  the  motion  of  all  components. 
The  large  space  structures  will  not  be  rigid,  instead  they  will  have  considerable  flexibility 
and  multiple  modes  of  vibration  [Ref.  1:  p.  51] 

Control  of  the  structure's  attitude  and  vibrations  requires  knowing  the  motions  of 
the  components.  One  approach  would  be  to  heavily  instrument  the  space  structure,  but 
weight  and  cost  make  this  approach  impractical.  An  alternative  is  to  use  a  limited 
number  of  sensors  to  measure  only  certain  states  and  to  deduce  the  other  required  states 
by  use  of  an  observer  algorithm. 

This  thesis  will  address  the  production  of  estimates  of  the  states  needed  for  control 
of  the  structure.  The  model  used  will  be  a  early  design  study  by  McDonnell  Douglas 
Astronautics  for  a  dual  keel  space  station.  The  techniques  and  problems  of  observation 
for  this  model  are  generic  to  all  large  space  structures. 

B.  PROBLEM  STATEMENT 

Design  of  an  observer  for  estimating  the  states  of  a  large  space  structure  breaks 
down  into  several  steps.  First,  a  matematical  model  is  developed  for  the  system  behavior 
over  time.  Modal  analysis  is  used  to  form  a  system  composed  of  decoupled  second  order 
differential  equations.  The  use  of  decoupled  equations  allows  a  reduced  order  model  to 
be  generated  by  truncating  the  number  of  modal  equations.  A  reduced  order  model  will 
have  all  of  the  same  mathematical  qualities  (and  problems)  but  reduces  the  amount  of 
time  and  computer  resources  required  to  do  simulation. 

Second,  the  observer  is  designed.  The  observer  is  designed  to  obtain  a  minimum 
variance  estimate  of  the  desired  state  values  from  the  measurements. 

Third,  the  observer  is  simulated  to  verify  performance.  Simulation  runs  of  both  a 
matched  observer/plant  system  and  a  reduced  order  observer  are  employed.  That  is,  the 
system  is  run  where  the  observer  is  used  to  estimate  all  of  the  plant  states  and  run  where 
there  are  more  plant  states  than  the  observer  estimates. 


Fourth,  results  are  analysed  and  conclusions  drawn  based  on  these  results. 
Reccomendations  for  further  areas  of  research  are  suggested  based  on  the  results  and 
conclusions. 

C.     ORGANIZATION 

The  model  of  the  space  station  is  developed  in  Chapter  II.  The  modal  model  was 
developed  using  modal  analysis  and  discretized  to  form  the  discrete-time  state  equations. 
The  data  for  this  model  was  from  an  early  design  study  by  McDonnell  Douglas 
Astronautics  Company  for  a  dual-keel  space  station.  The  observer  and  its  equations  are 
developed  in  Chapter  III.  Chapter  IV  is  the  simulation  runs  of  the  observer  versus  the 
plant.   Chapter  V  presents  conculsions  and  recommendations  for  further  research. 


II.     MATHEMATICAL  MODEL 

A.  INTRODUCTION 

Prior  to  the  proposed  space  station  almost  all  of  the  objects  put  into  space  could 
be  treated  as  simple  rigid  bodies  for  the  purpose  of  mathematical  modelling  of  their 
motions.  The  design  constraints  imposed  by  the  high  cost  of  lifting  mass  to  orbit  dic- 
tates a  light,  open  structure  with  considerable  flexing.  Large  space  structures  such  as  the 
space  station,  therefore,  cannot  be  treated  as  rigid  bodies.  The  structure  is  in  fact  lightly 
damped  with  multiple  natural  frequencies.  The  result  is  a  structure  that  will  vibrate  for 
considerable  periods  of  time  whenever  external  forces  are  applied. 

The  space  station  structure  can  be  modeled  as  an  n-DOF  (degree  of  freedom)  system 
consisting  of  n  masses,  springs,  and  dashpots  [Ref.  2:  p.  173-176].  This  straight  forward 
modelling  of  the  coupled  masses  produces  a  system  of  unworkable  complexity.  As  a 
result,  the  system  will  be  modelled  in  terms  of  the  structures  natural  modes  of  vibration. 
The  resulting  system,  while  still  complex,  is  at  least  workable. 

The  model  will  be  developed  in  two  steps.  The  first  will  be  to  generate  the 
continous-time  model  of  the  natural  modes.  The  second  will  yield  the  discrete-time 
model,  developed  from  the  first  model,  for  use  in  the  simulation. 

B.  MODAL  MODEL 

The  space  station  structure  can  be  modeled  as  a  system  of  discrete  masses  coupled 
by  springs  and  dashpots.  The  major  mechanism  of  damping  in  the  structure  is  structural 
damping,  the  internal  dissipation  of  energy  within  the  members,  as  the  structure  vibrates. 
Structural  damping  can  be  shown  to  be  equivalent  to  viscous  damping  and  this  equiv- 
alency is  used  in  the  model  [Ref.  2:  p.  72-73]. 

The  energy  dissipated  by  structural  damping  is: 

Wd=a.X2  (1) 

Wd    =  energy  dissipated  by  structural  damping 
a     =  constant  (force/displacement) 
X     =  displacement 

The  energy  dissipated  by  viscous  damping  is: 

Wv  =  nccoX2  (2) 


We  can  equate  the  two 


nCeqcoX  =  aX  (3) 


yielding  an  equivalent  viscous  damping  coefficient: 

C    =-£-  (4) 

The  second  order  differential  equation  for  a  single  viscously  damped  mass  is: 

mx  +  ex  +  kx  =  F{t)  (5) 

Substituting  Ceq  for  c 

mx  +  -^x  +  kx  =  F{t)  (6) 

For  multiple  mass  systems  Ceq  becomes  —  K  where  a>f  is  the  natural  frequency  of  vi- 
bration. 

The  displacement  of  masses  can  be  represented  by  the  second  order  matrix  differ- 
ential equation  [Ref.  3:  p.  3-9], 

Mq(t)  +  4jMt)  +  Kq(<)  =  m  (7) 

q        =  coordinate  vector 

M         =  system  mass  matrix  (diagonal) 

-rr-  K    =  equivalent  damping 

d        =  damping  coefficient 

co7       =  frequency  of  oscilation  of  the  system 

K       =  symmetric  system  stiffness  matrix 

F(0     =  system  forcing  function 

The  above  equation  represents  a  system  of  second  order  differential  equations  cou- 
pled through  the  stiffness  matrix.  Decoupling  can  be  done  by  expressing  q  in  terms  of 
natural  modes  of  vibration.  The  process  is  called  modal  analysis.  The  independent  dif- 
ferential equations  can  then  be  treated  individually.  The  modal  equations  are  derived 
below. 

First,  the  undamped,  homogeneous  form  of  Eq.  (7) 


Mq(t)  +  Kq(t)  =  0  (8) 
is  solved.    Let 

q{t)  =  Ax  s'm(cot  +  0)  (9) 

q{t)  =  Axco  cos(a)t  +  0)  (10) 


q\t)  =  -  Axoj2  s'm{(ot  +  0)  (11) 


substituting  Eq.  (9)  and  Eq.  (10)  into  Eq.  (11) 


[  -  co2M  +  K]Ajc  sin(co/ +  0)  =  0  (12) 

This  equation  has  a  non-trivial  solution  for  all  time  if  and  only  if: 

[K  -  w2M]x  =  0  (13) 

Equation  (12)  has  n  combinations  of  x  (natural  mode  shapes)  and  co  (natural  fre- 
quencies) as  solutions.   These  can  be  grouped  into  matrices: 

X  =  [^2...^]r  (14) 

Q.2  =  diag[a)20^2o2...(oln]  (15) 


which  satisfy  the  equation: 


KX  =  Q2MX  (16) 


Several  useful  relations  can  be  derived  from  Eq.  (16).  Premultiplying  Eq.  (16)  by 

XrKX  =  Q2XrMX  (17) 
The  eigenvectors  can  be  normalized 

XrMX  =  I  (18) 
which  yields 

XTKX  =  Q2  (19) 


The  equations  of  motion  can  be  uncoupled  through  the  linear  transformation  of  the 
coordinate  system 

n 

<?(')  =  X^(')  =  X>K0  (20) 

(=1 

X     =    modal  matrix 

n     =    maximum  number  of  degrees  of  freedom 

rj(t)    =    transformed  coordinate  vector 

Application  of  the  transformation  to  the  system  Eq.  (7)  yields 

XTMXij(r)  +  ~-  XrKX//(/)  +  XTKXri(t)  =  XTF{r)  (21) 

Using  Eq.(18)andEq.  (19) 

XTMXij{t)  =  liftt)  =  n  (22) 


d-XTKXn{t)  =  4-f 


-§-  XTKXr,{t)  =  -§-  Q^(f)  =  d£Ln  (23) 


therefore 


XrKX^(/)  =  Q}ri  (24) 


n  +  dQ.ii  +  Cl2n  =  XTF  (25) 


Equation  (25)  is  the  modal  model  of  uncoupled  second  order  differential  equations.   The 
motion  of  the  structure  can  be  found  from  the  modal  amplitudes,  r\{t),  using  Eq.  (20). 

C.     DISCRETE-TIME  MODEL 

The   discrete-time   state   space  model  is  found  by  solving  the  continuous-time 
equations.   The  ith  equation  of  motion  is 

Ut)  +  do>Mt)  +  olfiit)  =  x[T(t)  (26) 

Xf   =    transpose  of  the  ith  mode  shape  vector 
F(r)    =    torquing  force  applied  at  a  point 

The  homogeneous  solution  (F(f)  =  0)  for  Eq.  (26)  is  [Ref.  4:  p.  475-476] 


Vi{t)  =  C,e  y  cos(codt)  +  C2e  yl  sin(w/) 


(27) 


where 


dco, 


y  = 


(28) 


co, 


=  yjvli  ~ 


The  constants  in  Eq.  (27)  can  be  found  by  taking  the  derivative 

n{t)  =  {C2cod  -  Cxy)e~yt  cos(codt)  -  {Cxcod  -  C2y)e~yt  sm(codt) 


and  evaluating  at  t  =  0 


Solving  for  C,  and  C, 


»/(0)  =  C, 


J7(0)  =  C2o)d-  Qy 


C,  =  u(0) 


^(0)_       ^(0)y 

V-T    —        ,.,  + 


CO, 


CO, 


In  matrix  form 


G 


l 

y 


o 
^  j 


H(0) 

*(0) 


Rewriting  Eq.  (27)  and  Eq.  (30)  in  matrix  form 


(29) 


(30) 

(31) 

(32) 

(33) 
(34) 


(35) 


nit) 

m 


e  yl  cos(codt)  e  yt  sin(a>/) 

e~yt[y  cos{codt)  +  cod  sin(co/)]    e~yt[cod  cos{codt)  -  y  sin(a>/)] 


C 


(36) 


Substituting  Eq.  (35)  into  Eq.  (36),  the  solution  can  be  written  in  terms  of  the  initial 
conditions 


to) 


e  Y  [  cos(ov)  +  —  sin(codr)] 


CO 


CO, 


°j-vl 


—  e  ytsm{codt) 


dTyrsin(co/)  e  y  [  cos(co/)  -  —  sin(ov)] 


co^ 


1/(0) 

'7(0) 


(37) 


Letting 


xxo  = 


WO 
WO 


(38) 


and 


A;  = 


e_y,[  cos(coy)  +  —  sin(ov)] 


^dv  ■    Jjsin(aV)]  lo7e  y/ sin(co^) 

-~  e~yl  sin(co/)  e~y'[  cos(ov)  -  —  sin(co/)] 


(39) 


the  solution  can  be  written  as 


Xfit)  -  A,(/)X,(0) 


(40) 


where  A,  is  the  state  transition  matrix  of  the  ith  mode.  The  non-homogeneous  solution 
is 


X/(r)  =  Ai(r)X/(0)  +  B^/F(0) 


(41) 


where  the  discrete-time  input  matrix,  for  constant  F,  is  given  by 


?t 


B,= 


B/t^t 


(42) 


and  r  =  [0  \y  is  the  input  matrix  for  the  continuous-time  system,  and  T  is  the  sampling 
time.    Solving  Eq.  (42)  yields 


B,= 


1 


co. 


-ov)-^-- 
-tj7  e~yt  sin(codf) 


[\-ey  cos(gv)  -  —  e  y  sin(co/)] 


1 


(43) 


The  discrete-time  state  equation  for  the  ith  equation  of  motion  can  be  written  as 


X,{kT+  1)  =  Atf)XJik7)  +  BiiTjx/YikT) 


(44) 


where  A,  and  B,  are  evaluated  at  /  =  T .   Here, 
X,    =    vector  of  the  ith  modal  amplitude  and  the  ith  modal  velocity 


A,  =  ith  state  transition  matrix 

B,  =  ith  input  vector 

.v,r  =  transpose  of  the  ith  mode  shape  vector 

F  =  distrubuted  force  on  the  plant 

T  =  sampling  time 

k  =  time  index 

Equation  (44)  can  be  expanded  to  include  the  disturbance  input,  w{kT)  : 

Xi(kT+  1)  =  X£T)X£kT)  +  Bf(7>/[F(*7)  +  w(A7)]  (45) 

Equation  (45)  is  the  discrete-time  mathematical  model  describing  the  motion  of  the 
structure  in  terms  of  its  natural  modes  of  vibration. 


III.     THE  OBSERVER 

A.  INTRODUCTION 

The  observer  design  will  be  required  to  estimate  the  modal  states  in  a  noisy 
enviroment.  Kalman  filtering  is  the  most  widely  used  technique  for  accomplishing  the 
production  of  state  estimates  in  a  noisy  enviroment  [Ref.  5:  p.  159].  The  steady  state 
Kalman  filter  was  selected  to  minimize  the  computations  during  the  actual  plant  ob- 
server operation.  Use  of  a  steady-state  gain  matrix  for  the  observer  allows  the  matrix 
to  be  computed  separately  from  the  operational  observer,  reducing  the  computer  power 
required  for  the  observer  and  allowing  the  algorithm  to  operate  more  rapidly. 

B.  KALMAN  FILTER  EQUATIONS 

The  discrete  Kalman  filter  provides  state  estimates  for  the  following  dynamic  sytem 
[Ref.  5:  p.  159-162], 

X(*  +  1 )  =  AX(A)  +  BU(£)  +  BnW(A)  (46) 

Y(*  +  1)  =  CX(ft  +  1)  +  V(k  +  1)  (47) 

X  =  n  x  1  state  vector 

U  =  P  x  1  control  vector 

W  =  r  x  1  plant  noise  vector 

Y  =  m  x  1  measurement  vector 

V  =  raxl  measurement  noise  vector 
A  =nxn  state  transition  matrix 

B     =  n  x  p    control  input  matrix 
Bn    =  n  x  r    plant  noise  input  matrix 
C     =  m  x  n    measurement  matrix 

The  plant  noise  vector  W(A)  is  gaussian  white  noise  with 

E{W(/c)}  =  0  (48) 

E{W(A)Wr(/c)}  =  Q  (49) 
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for  all  k  =  0,1,2,...  ,  and  Q  is  a  positive  semi-definite  r  x  r  matrix.  V(A)  is  gaussian  white 
noise  with 

E{V(*)}  =  0  (50) 

E{V(/c)Vr(/c)}  =  R  (51) 

for  all  k  =  0,1,2,...  ,  and  R  is  a  positive  definite  m  x  m  matrix.  The  two  random  processes 
W(/c)  and  V(/c)  are  assumed  to  be  independent,  so  that 

E{V(/)W(/c)}  =  0  (52) 

for  ally  =  1,2,...  ,  and  k  =  0,1,2,...  The  intial  state  X(0)  is  assumed  to  be  a  gaussian  ran- 
dom vector  with 

E{X(0)}  =  0  (53) 

It  is  assumed  that  X(0)  is  independent  of  W(/c)  and  \(k). 

The  optimal  estimate  of  X{k  +  1)  is  denoted  X(k  +  1  |  k+  1).    The  Kalman  filter  is 
designed  to  minimize 

J  =  E{[X(£  +  1)  -  X{k  +  1 1  k  +  l)]r[X(A  +  1)  -  X{k  +  1  |  k  +  1)]}  (54) 

The  recursive  realtions  for  generating  X(A:  +  1  |  k  +  1)  are 

X{k+\\k)  =  AX{k  |  k)  +  BU{k)  (55) 

X(k  +  \  \  k  +  \)  =  X{k  +  \  \  k)  +  G(k  +  1)[Y(*  +  1)  -  CX(*  +  1  |  *)]  (56) 

for  k  =  0,1,2,...  ,  where  X(0  I  0)  =  0.  X(0  |  0)  is  set  equal  to  zero  since  the  expectation  of 
X(0)  is  zero. 

G(/c  +  1)  is  an  n  x  m  matrix  ,  called  the  Kalman  Gain  Matrix  which  is  specified  by 
the  realtions: 

?(k  +  1  |  k)  =  AP(*  |  k)AT+  BnQ(A)Bnr  (57) 

G(*  +  1)  =  ?{k  +  1  |  k)CTlCP(k  +  1  |  k)CT+  R(k  +  1)]_1  (58) 

P(A  +  1  |  k  +  1)  =  [I  -  G(k  +  l)C]P(/c  +  1  |  k)  (59) 
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P(/<  I  k)  is  the  covariance  matrix  of  the  error  between  the  states  and  their  estimates 

P(/c  |  k)  =  E{[X(*)  -  X(k  |  k)-][X(k)  -  X(k  |  *)]r}  (60) 

Since  we  are  using  the  steady  state  gains  the  choice  of  P(0  |  0)  is  irrelevant.    P(0  |  0)  is 
initialized  to  zero  in  the  gain  derivation  program  for  simplicity  [Ref.  6:  p.  139-140]. 

C.    STEADY-STATE  SOLUTION 

If  Equations  (57),  (58),  and  (59)  are  repeatedly  iterated,  G(k  +  1)  will  converge  to  a 
steady  state  value  [Ref.  7:  p.  263]. 


Gss=     lim    G{k+  1)  (61) 

k  ->  oo 

The  values  of  G„  (or  G)  can  be  substituted  into  Eq.  (56)  making  the  steady  state 
Kalman  filter 

X(k  +  1  |  k)  =  AX{k  |  k)  +  BU(&)  (62) 


X(/<  +\\k+l)  =  X{k+\\k)  +  G[Y(A  +  1)  -  CX(A  +  1  |  /<)]  (63) 

D.     OBSERVER  PERFORMANCE 

The  performance  of  an  observer  is  judged  by  how  accurately  and  rapidly  it  estimates 
the  desired  states.  The  performance  measure  of  the  observer  as  a  whole  is  shown  in 
equation  (54).   The  normalized  performance  of  the  observer  for  individual  states  is 

Jj-ECfo-Sjft/EDr?]  (64) 

which  can  be  found  using  Eq.  (65) 


J,-  =  2W*)  "  *i{k))2T*  *  //ft*)7*  (65) 

k=0  fc=0 

J,        =  performance  measure  for  the  itfi  state 

x,(k)      =  value  of  the  ith  state  at  k 

x,{k)      =  observer  estimateof  ith  state  at  k 
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Ts        =    sample  interval 

A  normalized  performance  measure  is  used  to  aid  comparison  of  the  performance  of  the 
observer  in  estimating  various  states.  From  Eq.  (65)  it  can  be  shown  that  if  x,(k)  =  0  for 
all  k  =  0,1,2,...  that  J,  would  be  unity.  Therefore,  the  better  the  performance  of  the  ob- 
server, the  smaller  the  fraction  of  one  J,  will  be. 
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IV.     SIMULATION 

A.  INTRODUCTION 

The  objectives  of  the  simulation  were  to 

•  determine  the  sensitivity  of  the  observer  performance  and  settling  time  to  changes 
in  the  ratio  of  plant  noise  to  measurement  noise, 

•  determine  the  effect  on  observer  performance  and  settling  time  of  increasing  the 
number  of  modes  observed  in  the  matched  plant/observer,  and 

•  determine  the  performance  for  the  reduced  order  observer. 

B.  PLANT  AND  OBSERVER  DATA 

The  dynamic  model  is  a  truncated  form  of  a  preliminary  space  station  configuration; 
the  phase  II  dual  keel  structure.!  The  full  model  consists  of  an  infinite  number  of  natural 
modes  but  this  was  restricted  to  the  first  ten  active  modes  for  this  study  due  to  limita- 
tions on  computer  resources.  As  will  be  shown  reasonable  data  can  be  obtained  with  this 
simplification  in  examining  the  observer  performance. 

C.  SIMULATION  PROGRAMS 

The  simulation  was  broken  down  into  two  segments  due  to  the  large  memory  and 
computational  time  requirements.  The  first  program  computed  the  steady  state  observer 
gain  matrix  (G).  The  second  program  ran  the  observer  and  the  plant  when  the  plant 
was  subjected  to  an  impulse  excitation. 

The  steady  state  observer  gain  matrix  (G)  was  obtained  by  repeated  iteration  of 
equations  (57),  (58),  and  (59).  The  equations  were  were  run  until  the  values  of  the  ma- 
trix changed  by  less  than  a  set  fraction.  The  following  formula  was  used  to  check  the 
changes  in  the  gain  matrix  elements 

Agv  =  [&/*  +  1 )  -  gijiWIgijik  +  1 )  (66) 

The  program  was  terminated  when  8gtJ  was  less  than  lO-10 . 

The  settling  time  for  the  estimates  of  the  states  to  be  within  2%  of  the  actual  states 
was  determined  by  finding  the  eigenvalues  of  A  -  G*C  then  computing  as  follows  [Ref. 
6:  p.  139-143] 


1   The  model  for  preliminary  station  configuration  was  provided  courtesey  of  McDonnel 
Douglas  Astronautics  Company,  5301  Bolsa  Avenue,  Huntington  Beach,  CA  92647. 
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?;  =  log(.02)/log(ii4aCmin)  (67) 

The  expected  error  in  the  sensor,  i.e.,  the  standard  deviation  of  the  noise  in  the 
measurement,  was  choosen  as  10"3  feet  per  sec.  per  sec.  based  on  the  natural  frequencies 
in  the  structure  and  reasonable  sensor  sensitivity  [Ref.  2:  p.  79-80].  The  expected  plant 
noise  was  varied  to  find  the  range  of  ratios  between  plant  and  measurement  noise  that 
the  filter  would  be  effective.  This  approach  was  taken  since  the  plant  noise  contributors 
are  not  currently  well  defined. 

The  second  program  subjected  the  plant  as  modeled  in  Eq.  (45)  to  an  impulse 
excitation  and  then  had  the  observer  estimate  the  selected  states  using  observer 
equations  (62)  and  (63).   Observer  performance  was  computed  using  Eq.  (65). 

A  third  program  was  used  to  find  the  contribution  of  unobserved  modes  to  the  noise 
in  the  Kalman  observer.  The  program  ran  the  plant  subject  to  an  impulse  excitation  and 
computed  the  product  of  the  measurement  matrix  C  times  the  unobserved  modes  of  the 
state  vector  X(k)  for  a  measure  of  the  noise  contributed  by  the  unobserved  modes. 

The  three  programs  are  listed  in  the  appendices. 

D.     EFFECT  OF  PLANT  TO  MEASUREMENT  NOISE  RATIO  ON  OBSERVER 
PERFORMANCE 

The  ratio  of  the  variance  of  the  plant  noise  (PN)  to  the  variance  of  the  measurement 
noise  (MX)  was  found  to  have  a  strong  effect  on  the  Kalman  Observer  performance  (J) 
and  settling  time  (Ts)  .  Figures  1  through  6  show  the  observer  performance  for  a  3  mode 
matched  plant  and  observer  system  for  progressively  smaller  PN/MN  ratios. 2  Figure  7 
shows  the  performance  for  the  seventh  mode  (position)  versus  several  values  of  PN/MN. 
Figure  8  is  the  settling  time  versus  the  same  PN/MN  ratios. 

The  figures  show  that,  for  all  of  the  plotted  performance  values,  the  observer  per- 
formance is  at  least  marginally  acceptable  regardless  of  the  PN/MN  ratio.  Decreasing 
the  PN/MN  ratio  leads  to  an  even  more  rapid  degradation  in  observer  performance. 
The  settling  times  also  rapidly  increase  as  the  PN/MN  ratio  decreases. 


2  Figures  1-6  and  9-15  show  the  performance  measure  for  each  mode.  The  bar  for  the  mode 
position  is  immeadiately  to  the  right  of  the  numbered  tick  mark  on  the  x-axis  scale,  the  mode  ve- 
locity is  next  to  it  immeadiately  adjacent  to  the  tick  mark  without  a  number. 
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E.  EFFECTS  OF  INCREASED  MODES  ON  OBSERVER  PERFORMANCE 

The  matched  plant/observer  was  run  with  increasing  numbers  of  modes  to  see  if 
there  was  an  effect  on  observer  performance  (J)  or  settling  time  Ts  .  Figures  7  through 
17  are  of  observer  performance  for  systems  with  increasing  numbers  of  modes  in  the 
system  being  observed.  Figure  18  is  of  settling  time  versus  the  number  of  modes  in  the 
system.   The  ratio  of  PN/MN  was  kept  constant  at  PN/MN  =  2.5  x  109. 

The  increasing  of  the  number  of  modes  for  the  matched  plant/observer  had  neglible 
effect  on  the  performance  for  the  individual  modes.  The  performance  value  for  the 
modes  was  effectively  constant.  Settling  times  for  the  observers  increased  as  the  number 
of  modes  was  increased. 

F.  REDUCED  ORDER  KALMAN  OBSERVER 

The  Kalman  Observer  has  been  shown  to  be  effective  where  the  number  of  modes 
observed  matches  the  number  of  modes  in  the  plant.  The  Kalman  Observer  was  then 
run  with  the  one  less  mode  observed  than  the  number  of  modes  in  the  plant.  The  gain 
matrix  (G)  from  the  matched  system  was  used.  The  observer  failed  with  the  state  esti- 
mates produced  by  the  observer  becoming  excessively  large  and  having  settling  times  of 
hours  vice  minutes.  Since  the  purpose  of  the  observer  was  to  provide  estimates  for  use 
in  controlling  the  plant  the  time  delay  makes  the  estimates  unusable. 

The  cause  of  the  observer  failure  is  apparent  when  you  look  at  the  last  portion  of 
Eq.  (56)  of  the  Kalman  Observer 

G[Y(/c+l)-CX(/c+  1  |  *)]  (68) 

This  portion  of  the  observer  equation  is  the  correction  of  X(k  +\\k)  to  produce 
X{k  +  1  |  k  +  1)  .  The  design  of  the  Kalman  observer  is  to  produce  an  estimate  despite 
the  measurement  noise  but,  with  the  reduced  order  filter  there  is  additional  unanticipated 
noise  which  causes  over  correction  of  the  values  of  Pleading  to  the  state  estimates  being 
excessively  large  and  settling  times  being  too  long.  This  can  be  shown  by  examining 
what  composes  Y(k  +  1)  -  CX(k  +  1  |  k) 


16 


" 

*,(*) 

- 

x2(k) 

*l(*) 

T 

*iW 

1 

T 

xm-i(k) 

1 

xm(k) 

*m-i(*) 

— 

-c 

*m(*) 

*/n+l(*) 

— 

■*/?1+2W 

0 

T 

T 

1 

i 

x„-\(k) 

0 

*»(*) 

- 

-                       - 

(69) 


C  times  the  state  xmM(k)  through  xn{k)  is  unanticipated  noise  so  if 


*m 
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x2(k) 

x2(k) 

T 
I 

-C 

T 
1 

xm-i(k) 

4-i  (*) 

xm{k) 
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=  0 


(70) 


the  remaining  portion  of  the  C  matrix  times  the  modal  states  is  an  equivalent  noise. 

Table  (1)  shows  the  growth  of  the  unanticipated  noise  in  the  filter  as  the  number 
of  unobserved  modes  in  the  plant  grows.  Table  (2)  shows  the  individual  contributions 
of  the  individual  modes  when  left  unobserved.  Table  (1)  shows  that  the  unanticipated 
noise  is  much  larger  than  that  expected  by  the  filter  (10-3).  Table  (2)  shows  that  there 
are  modes  that  do  not  markedly  contribute  to  the  noise  and  that  they  might  successfully 
be  left  unobserved  if  the  measurement  noise  estimate  was  already  much  larger  than  these 
noise  sources. 
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Table  1.     CUMULATIVE    UNANTICIPATED   NOISE   FROM    UNOBSERVED 
MODES 


Num- 
ber of 
Unob- 
served 
Modes 

Unob- 
served 
Modes 

El 

E2 

E2 

1 

10 

0.647 

97.440 

3.277 

2 

10-11 

366.354 

95.764 

3.142 

-> 
j 

10-12 

366.355 

95.764 

3.142 

4 

10-13 

365.565 

95.855 

3.143 

5 

10-14 

365.426 

96.032 

5.704 

6 

10-15 

144195.7 

116.205 

2201.94 

7 

10-16 

148006.8 

170.142 

5475.21 

8 

10-17 

473974.3 

39424.1 

5692.50 

9 

10-18 

474344.2 

60078.8 

9419.77 

10 

10-19 

474358.5 

68987.7 

9865.27 

Table  2.     UNANTICIPATED    NOISE    FROM     UNOBSERVED    MODES    BY 
MODE 


Unobserved 
Mode 

El 

E2 

E3 

10 

0.64716 

97.4403 

3.27761 

11 

359.342 

0.20929 

0.21429 

12 

0.9353E-08 

0.1364E-07 

0.2196E-07 

13 

0.3S52E-02 

0.4409E-02 

0.901 7E-03 

14 

0.5615E-03 

0.2592E-01 

2.57554 

15 

143090.9 

17.9682 

2167.02 

16 

195.736 

83.3458 

5675.91 

17 

324471.2 

39252.26 

221.170 

18 

216.240 

21133.6 

3736.93 

19 

7.69298 

8829.95 

458.504 

20 

2.3194 

3949.16 

108.981 
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Figure  3.      Observer  Performance  (J)   PN/MN  =  l.OdlO 
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22 


in 

CM 

o 


PERFORMANCE  MEASURE  (J)  PN/MN=2.5D09 


MODE 


Figure  5.      Observer  Performance  (J)   PN/MN  =  2.5d09 


23 


a 

O 


PERFORMANCE  MEASURE  (J)  PN/MN=1.0D09 


MODE 


Figure  6.      Observer  Performance  (J)   PN/MN  =  1.0d09 


24 


CM 

' 

o 
o 

CN 
O 

1 

O 
t^    CD 

[J)    MOD 

012               0. 

CJ  ° 
ZI 

or 

O  oo 

\ 

PEF 

004               0. 

\ 

\ 

o 

o 

o 
o 

1 

\ 

l'o9 

1010 
PN 

1 

/MN 

R 

Mr 
=ITI 

10" 
0 

1 — 1 

10l 

2 

Figure  7.      Mode  7  (Postion)  Observer  Performance  versus  PN/MN 


25 


LJ 

i— i 
E-h 

CD 


LJ 
CO 


, , 

J 

s_ 

\ 

\ 

\ 

V 

*s 

N 

S. 

1 — 1 

S\, 

s 

s 

>fc_ 

_ 

N 

X 

s> 

'o.j 

\ 

\ 

' — 1  _ 

s 

\ 

\ 

\ 

^ 
\ 

s 

^   _ 

\ 

o 
CD 

•""' 

L 

1 

1 

1  oil 

1  1 

1 

4  ol2 

10 


10lu  10' 

PN/MN    RRTTn 


10' 


Figure  8.      Settling  Time  versus  PN/MN 


26 


Figure  9.      Observer  Performance  (J)  4  Modes  (7  -  10) 
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Figure  10.      Observer  Performance  (J)   5  Modes  (7  -  11) 
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Figure  11.      Observer  Performance  (J)  6  Modes  (7  -  12) 
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Figure  13.      Observer  Performance  (J)  8  Modes  (7  -  14) 
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Figure 


14.      Observer  Performance  (J)  9  Modes  (7  -  15) 
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Figure 


15.      Observer  Performance  (J)    10  Modes  (7  -  16) 
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Figure  16.      Settling  Time  versus  number  of  Modes  Observed 
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V.     CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

Simulations  runs  showed  that  a  matched  plant; observer  can  work  if  the  following 
criterions  are  meet: 

•  The  ratio  of  plant  noise  to  measurement  noise  is  sufficiently  high  to  produce  a  us- 
able settling  time. 

•  Sufficient  computational  power  is  available  to  run  the  matched  observer.  The 
amount  of  memory  and  number  of  computation  goes  up  as  the  number  of  modes 
observed  increases. 

Utilizing  a  reduce  order  observer  for  an  arbitrarily  selected  set  of  modes  is  not  fea- 
sible. The  non-observed  modes  add  so  much  noise  to  the  system  that  settling  times  and 
observer  performance  are  so  poor  as  to  render  the  observer  useless  for  obtaining  state 
values  for  plant  control. 

B.  RECOMENDATIONS 

The  work  on  the  Kalman  Observer  for  Large  Space  Structures  lead  to  the  following 
recommendations  for  further  research: 

•  Identify  those  modes  that  contribute  the  largest  noise  to  the  Kalman  observer  and 
set  the  observer  to  estimate  these  states  in  addition  to  those  required  for  plant 
control.  A  possible  method  for  identifying  the  modes  that  contibute  the  largest 
noise  to  the  observer  would  be  the  Karhunen-Loeve  expansion. 


• 


Modifying  the  plant/observer  to  model  the  use  of  sensors  at  additional  positions 
to  see  if  the  increase  in  the  data  rate  will  help  decrease  settling  time. 

•  Modify  the  model  to  incorporate  noise  injection  into  more  than  one  location  .  The 
current  model  has  noise  injected  at  only  one  position,  a  useful  simplification  for 
initial  analvsis  but  not  realistic. 
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APPENDIX      A.  KALMAN  GAIN  MATRIX  GENERATION  PROGRAM 


*********************************************************** 

*****  GGAIN  ***** 

*****  ADAPTED  TO  RUN  KALMAN  FILTER  AND  COMPUTE  THE  ***** 
*****  g  MATRIX  BY  ITERATION  STOPPING  WHEN  THE  ***** 
*****  THE  MATRIX  GOES  TO  STEADY  STATE  ***** 

*********************************************************** 


*********************************************************** 

*****  VARIABLE  DECLARATIONS  ***** 

*********************************************************** 

EXTERNAL  STMTRX,DLINRG,EXCMS,  DEVCRG 

CHARACTERS  NAM 

CHARACTER* 1  AGAIN , CORECT , RAGAIN 

INTEGER  R0WN1,R0WN2,R0WN3, COUNT, NODE, MODE, KQ,EMODE,SMODE,R2M,C2M 

INTEGER  CT,CF,KADJ,CFADJ,LOOP,PRNT,JJ,JK,Nl,JR,KR,MR,ISEED,M2 

INTEGER  JL,J1,JM 

REAL  LAMA(IOO),  UGVEX(684, 100) ,RNODE ,RMODE,MIN 

REAL*8  PHI(2,2,100),GAMMA(2,100),EGT,GMA,WN,W1,X1T,X2T,TIME 

REAL*8  A(200,200),B(200,3),F(3,  50) , IMPLSE, ENERGY 

REAL*8  C0SW1T,SINW1T,X1( 100) ,X2( 100) ,COST( 100) 

REAL*8  DAMP , SAMPT , PI , SAMPTM , SUM1 , SUM2 , SUM3 , SUMC 

REAL*8  C(6,200),  IDENT(  50,  50),  RMN(6,6),  QPN(3,3) 

REAL*8  PK(  50,  50),  Y(6),  BN(200,3) 

REAL*8  PNVARX,  PNVARY,  PNVARZ 

REAL*8  MNVX1,  MNVY1,  MNVZ1,  SUM,  BQBT(50,50) 

REAL*8  TMP1(  50,3),  TMP2(3,3),  TMP3(  50,  50) 

REAL*8  PK1(  50,  50), G(  50,3) 

REAL*8  DY(3),  ES,ED,ESUM,CGN,PRT 

REAL*8  SF,  N9,  TCHK,  ACHK,  HI,  H2,  H3,  H4,  H5 ,  H6 

REAL*8  AGC(100,100) 


COMPLEX*8  EVAL(IOO),  EVEC  (100,100) 


*********************************************************** 

*****  VARIABLE  DEFINITIONS  ***** 

**************************y?yc******************************* 

STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 

LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 

UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 

PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 

GAMMA  =  INPUT  TRANSITION  MATRIX 

A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 

B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 


GMA00010 
GMA00020 
GMA00030 
GMA00040 
GMA00050 
GMA00060 
GMA00070 
GMA00080 
GMA00090 
GMA00100 
GMA00110 
GMA00120 
GMA00130 
GMA00140 
GMA00150 
GMA00160 
GMA00170 
GMA00180 
GMA00190 
GMA00200 
GMA00210 
GMA00220 
GMA00230 
GMA00240 
GMA00250 
GMA00260 
GMA00270 
GMA00280 
GMA00290 
GMA00300 
GMA00310 
GMA00320 
GMA00330 
GMA00340 
GMA00350 
GMA00360 
GMA00370 
GMA00380 
GMA00390 
GMA00400 
GMA00410 
GMA00420 
GMA00430 
GMA00440 
GMA00450 
GMA00460 
GMA00470 
GMA00480 
GMA00490 
GMA00500 
GMA00510 
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c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


TCX,  TCY,  TCZ  =  CONTROL  TORQUE  VALUES 

ENERGY  =  TOTAL  SYSTEM  ENERGY 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 
ROWN1  =  X-SLOPE  LOCATION 
ROWN2  =  Y-SLOPE  LOCATION 
ROWN3  =  Z- SLOPE  LOCATION 
C  =  OUTPUT  MATRIX  FOR  Y 
IDENT  =  IDENTITY  MATRIX 

RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 
QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 
PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 
PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 
PNVARZ  =  PLANT  NOISE  Z -SLOPE  VARIANCE 
MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 
MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 
MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE 
I  SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 
XKAL  =  X  MATRIX 
Y  =  OUTPUT  MATRIX 
RNDM  =  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 

IN  PLANT  FORCES 
BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 
TNX,TNY,TNZ=  NOISE  TORQUES  X,Y,Z  SLOPES 
M2=2*MODE 


■J -  - '  -  - '  -  -•-  J -  JL  J-  JL  . '  - 


SAMPLE  OF  SPACE  EXEC  FILE 


y .  - '  -  *.'-  y -  J -  -'  -  J  -  J-  J  -  j<  -  J'  -  J  -  »•  -  .  • , . '  -  j - .  ■ . .  ■  - 


THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 


FORTVS  SPACE 

SPACE 

LOAD  SPACE  (START 


(COMPILES  PROGRAM) 

(EXECUTES  EXEC  FILE) 

(LOADS  AND  EXECUTES  PROGRAM) 


SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 


FI  4 
FI  8 
FI  11 
FI  13 
FI  14 
FI  16 
FI  17 
FI  18 
FI  19 
FI  20 


DISK  THESIS  INPUT  B  (PERM 

DISK  UTILITY  DATA   (RECFM  VS  BLOCK  133  PERM 

(RECFM  F  BLOCK  80  LRECL  80  PERM 
(RECFM  VS  BLOCK  133  PERM 
(RECFM  F  BLOCK  80  LRECL  80  PERM 
(RECFM  F  BLOCK  80  LRECL  80  PERM 


DISK  CNTRL  OUTPUT 
DISK  GAMMA  OUTPUT 
DISK  MODE  OUTPUT 
DISK  COST  OUTPUT 
DISK  PRT  OUTPUT 
DISK  ERROR  DATA 


(RECFM 
(RECFM 


DISK  END  FILE   (RECFM  F 


F  BLOCK  80  LRECL  80  PERM 
F  BLOCK  80  LRECL  80  PERM 
BLOCK  80  LRECL  80  PERM 


DISK  GMAT  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 


GMA00520 
GMA00530 
GMA00540 
GMA00550 
GMA00560 
GMA00570 
GMA00580 
GMA00590 
GMA00600 
GMA00610 
GMA00620 
GMA00630 
GMA00640 
GMA00650 
GMA00660 
GMA00670 
GMA00680 
GMA00690 
GMA00700 
GMA00710 
GMA00720 
GMA00730 
GMA00740 
GMA00750 
GMA00760 
GMA00770 
GMA00780 
GMA00790 
GMA00800 
GMA00810 
GMA00820 
GMA00830 
GMA00840 
GMA00850 
GMA00860 
GMA00870 
GMA00880 
GMA00890 
GMA00900 
GMA00910 
GMA00920 
GMA00930 
GMA00940 
GMA00950 
GMA00960 
GMA00970 
GMA00980 
GMA00990 
GMA01000 
GMA01010 
GMA01020 
GMA01030 
GMA01040 
GMA01050 
GMA01060 
GMA01070 
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5 

C 

1001 

1002 

1008 

C 

500 

C 

C 

C 


700 

c 
c 
c 
c 

c 

c 

701 

C 

c 
c 


READING  LAMA  AND  UGVEX  MATRICIES' 


y-  y-  y-  y-  y-  y-  y-  y-  - '-  -;-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  -J-  y-  y-  y  -  y-  y-  y-  y-  y-  y-  y-  y-  Vr  Vr  Vr  *V  *V  *VVc  V*  V'  V'  ~'r  Vr  V*  Vr  *V  Vr  Vc  Vr  Vr  Vr  Vr  Vc  V?  ~V  Vr  V? 

PARAMETER   (JR=5243,    KR=5397,    MR=262139) 

MIN  =1200.  0 

WT=1.  0D00 

PI   =  4.  ODO  *  ATAN(1.  ODO) 

*********************************************************** 

*****  READ  LAMA  AND  UGVEX  MATRICIES  ***** 

y  .  y-  y-  y-  y.  j-  y-  y-  -'-  y-  y-  y-  y.  y-  ju  y-  y-  y-  y-  y-y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y  -  y-  y-  y-  y  -  y-  y-  y-  y-  y-  y-  y-  yf  yf  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y-  y- 

CALL  EXCMS   ('CLRSCRN') 

WRITE(6,1008) 

WRITE(6,*)    ' 

WRITE(6,*)    '    * 

THIS   SECTION  READS  THE   LAMA  VECTOR  AND  THE  UGVEX 

MATRIX  AND  STORES  THEM   IN  MEMORY  FOR  FURTHER  RECALL  OF 

DESIRED   LOCATION  DATA. 


READ(4,1001)   NAM 

READ( 4 , 1002 ) ( LAMA( I ) , 1=1 , 100) 

READ(4,1001)   NAM 

DO  5   J  =   1,100 

READ(4,1002)(UGVEX(I,J),I=1,684) 
CONTINUE 

F0RMAT(1X,A6) 
F0RMAT(1X,8E15. 8) 
F0RMAT(1X,////) 

CALL  EXCMS   ('CLRSCRN') 

************  STARTING  MODE  NUMBER 

**   SMODE   7  TO   100   (INTEGER)   **** 
SM0DE=10 

WRITE   (16,700)    SMODE 

FORMAT  ('    ',' STARTING  MODE  NUMBER:       ',12) 

***************  NUMBER  OF  MODES  TO  SCAN 

**  MODE    1  TO   93   (INTEGER) 

MODE=  3 

EMODE  =  SMODE  +  MODE   -    1 


************** 


************* 


WRITE   (16,701)   MODE 

FORMAT  ('    ',' NUMBER  OF  MODES  SCANNED:       ',12) 

************  NOISE   INPUT  POSITION  ***************** 

**  NODE   1  TO   114   (INTEGER)    (IF  0  THEN  NO  NOISE   INPUT) 
NODE=   8 


GMA01080 
GMA01090 
GMA01100 
GMA01110 
GMA01120 
GMA01130 
GMA01140 
GMA01150 
GMA01160 
GMA01170 
GMA01180 
GMA01190 
GMA01200 
GMA01210 
GMA01220 
GMA01230 
GMA01240 
GMA01250 
GMA01260 
GMA01270 
GMA01280 
GMA01290 
GMA01300 
GMA01310 
GMA01320 
GMA01330 
GMA01340 
GMA01350 
GMA01360 
GMA01370 
GMA01380 
GMA01390 
GMA01400 
GMA01410 
GMA01420 
GMA01430 
GMA01440 
GMA01450 
GMA01460 
GMA01470 
GMA01480 
GMA01490 
GMA01500 
GMA01510 
GMA01520 
GMA01530 
GMA01540 
GMA01550 
GMA01560 
GMA01570 
GMA01580 
GMA01590 
GMA01600 
GMA01610 
GMA01620 
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702 

C 

C 

C 

C 


900 
C 

703 
C 
C 
C 


704 
C 

c 
c 
c 
c 


c 

510 


WRITE   (16,702)   NODE 

FORMAT  ('    ','    NOISE  NODE   LOCATION:         \I5) 


*************  SAMPLING  TIME  ************** 

**  SAMPT  MUST  BE  LESS  THAN  OR  EQUAL  TO  SAMPTM  ** 
SAMPT  =  . 05 

SAMPTM  =   ((2. 0D0*PI)/SQRT(LAMA(EMODE)))/2.OD0 

IF  (SAMPT.  GE.  SAMPTM)   THEN 
SAMPT=SAMPTM 

END  IF 

WRITE   (16,900)   MIN 

FORMAT  ('    ',2X,'MIN:       ' ,F8. 3) 

WRITE   (16,703)   SAMPT 

FORMAT  ('    '  /SAMPLING  TIME:       '  ,D12.4) 


DAMPING  FACTOR 
DAMP   0.0  TO   1.0   (REAL*8) 
DAMP=.  01 

WRITE   (16,704)   DAMP 

FORMAT  ('    ', 'DAMPING  FACTOR:       \D12.4) 


***  PLANT  NOISE  VARIANCE  *** 

**  PNVARX,    PNVARY,    PNVARZ  GT  0.  0 

SF1=2.5D06 

PNVARX=1.0D00*SF1 
PNVARY=1. 0D00*SF1 
PNVARZ=1.0D00*SF1 


***  MEASUREMENT  NOISE  VARIANCE  *> 

**  MNVARX,   MNVARY,    MNVARZ   GT  0.  0 

SF=1.  0 

MNVX1=5. 5D-03*SF 

MNVY1=5.5D-03*SF 

MNVZ1=5.5D-03*SF 


************** 


CALL  EXCMS   ('CLRSCRN') 
WRITE   (6,1008) 
WRITE   (6,*)    ' 


y-  .j-  j-  -»-  -'-  .j-  ~'-  j-  Vf  V*  Vr 


ROWN3 
ROWN2 
ROWN1 
COUNT 


NODE*6 
(NODE*6) 
(NODE*6) 
0 


NOISE   INPUT  LOCATION 


PROGRAM  RUNNING' 
************ 


GMA01630 

GMA01640 

GMA01650 

GMA01660 

GMA01670 

GMA01680 

GMA01690 

GMA01700 

GMA01710 

GMA01720 

GMA01730 

GMA01740 

GMA01750 

GMA01760 

GMA01770 

GMA01780 

GMA01790 

GMA01800 

GMA0181O 

GMA01820 

GMA01830 

GMA01840 

GMA01850 

GMA01860 

GMA01870 

GMA01880 

GMA01890 

GMA01900 

GMA01910 

GMA01920 

GMA01930 

GMA01940 

GMA01950 

GMA01960 

GMA01970 

GMA01980 

GMA01990 

GMA02000 

GMA02010 

GMA02020 

GMA02030 

GMA02040 

GMA02050 

GMA02060 

GMA02070 

GMA02080 

GMA02090 

GMA02100 

GMA02110 

GMA02120 

GMA02130 

GMA02140 

GMA02150 

GMA02160 

GMA02170 


39 


45 
40 
C 


46 
47 
C 


48 
C 
C 
C 


58 
60 
C 


*********** 


INITIALIZE  MATRICIES 


**************** 


C 

9999 

C 

C 

C 

C 


90 

92 
94 


DO  40  I  =  1,3 

DO  45  J  =  1,3 

RMN(I,J)=0.0 

CONTINUE 
CONTINUE 

DO  47  1=1,50 

DO  46  J=l,50 

IDENT(I,J)=0.0 

PK(I,J)=0.  0 

CONTINUE 
CONTINUE 

DO  48  K=l,50 

IDENT(K,K)=1.  0 
CONTINUE 

***  INITIALIZE  RMN  AND  QPN  MATRICES  *** 

DO  60  1=1,3 

DO  58  J=l,3 

QPN(I,J)=0.  0 

CONTINUE 
CONTINUE 

RMN(1,1)=MNVX1** 2 

RMN(2,2)=MNVY1**2 

RMN(3,3)=MNVZ1**2 

QPN(1,1)=PNVARX**2.  0 

QPN(2,2)=PNVARY**2.0 

QPN(3,3)=PNVARZ**2.0 

FORMAT  ( '    ' , '         '  ) 

VrVv~V~*~"  V*~l*"V"V*VVt"VVfV*'ifrtftf'V'/f1*^^ 

*****  BEGIN  MAIN  PROGRAM  ***** 

CALL  STMTRX( EMODE , SMODE , SAMPT , DAMP , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+   R0WN1,R0WN2,R0WN3,BN) 


***  PRE -LOOP  PORTION  OF  KALMAN  FILTER 

JK=SMODE*2-2 

M2=2*M0DE 

DO  94  1=1,3 

DO  92  J=1,M2 
JL=JK+J 
SUM=0.  0 

DO  90  K=l,3 

SUM=SUM+QPN( I ,K)*BN( JL,K) 
CONTINUE 
TMP1(J,I)=SUM 
CONTINUE 
CONTINUE 


GMA02180 
GMA02190 
GMA02200 
GMA02210 
GMA02220 
GMA02230 
GMA02240 
GMA02250 
GMA02260 
GMA02270 
GMA02280 
GMA02290 
GMA02300 
GMA02310 
GMA02320 
GMA02330 
GMA02340 
GMA02350 
GMA02360 
GMA02370 
GMA02380 
GMA02390 
GMA02400 
GMA02410 
GMA02420 
GMA02430 
GMA02440 
GMA02450 
GMA02460 
GMA02470 
GMA02480 
GMA02490 
GMA02500 
GMA02510 
GMA02520 
GMA02530 
GMA02540 
GMA02550 
GMA02560 
GMA02570 
GMA02580 
GMA02590 
GMA02600 
GMA02610 
GMA02620 
GMA02630 
GMA02640 
GMA02650 
GMA02660 
GMA02670 
GMA02680 
GMA02690 
GMA02700 
GMA02710 
GMA02720 
GMA02730 


40 


96 

97 
98 
C 


99 

100 


9374 

9375 

C 

C 

C 

C 


9991 
C 

C 

C 


DO  98  1=1, M2 
JL=JK+I 

DO  97  J=1,M2 
SUM=0.  0 

DO  96  K=l,3 

SUM=SUM+BN(JL,K)*TMP1(J,K) 
CONTINUE 
BQBT(I,J)=SUM 
CONTINUE 
CONTINUE 

M2=2*MODE 

DO  100  1=1, M2 

DO  99  J=1,M2 

TMP3(I,J)=0.0 

CONTINUE 
CONTINUE 
JL=JK+M2 
DO  9375  1=1,3 

DO  9374  J=1,JL 

C(I,J)=C(I,J)*SF 

CONTINUE 
CONTINUE 

********************************************************** 

*****     THIS  SECTION  COMPUTES  THE  STATE  UPDATE     ***** 

********************************************************** 

ESUM=0. 0 

COUNT  =  0 

ENERGY  =  0. 0D0 

TIME  =  0.  0 

CGN=0.  0 

*****   SETS  LOOP  FOR  THE  ITERATIONS  NECESSARY  TO  OBSERVE  ***** 

*****  THE  SYSTEM  FOR  THE  NUMBER  OF  MINUTES  SPECIFIED     ***** 

LOOP  =  INT((MIN*60.0)/SAMPT) 

PRT=(DBLE( LOOP))/ 1200.  0 

PRTA=(DBLE(LOOP))/2400.  0 

CNTA=0.  0 

ACHK=1.0D-10 

H1=0.  0 

H2=0. 0 

H3=0.  0 

H4=0. 0 

H5=0.  0 

H6=0.  0 

TCHK=MIN*60. 0 

CONTINUE 

TIME  =  TIME+   SAMPT 

CGN=CGN+1.  0 
CNTA=CNTA+1.  0 


GMA02740 
GMA02750 
GMA02760 
GMA02770 
GMA02780 
GMA02790 
GMA02800 
GMA02810 
GMA02820 
GMA02830 
GMA02840 
GMA02850 
GMA02860 
GMA02870 
GMA02880 
GMA02890 
GMA02900 
GMA02910 
GMA02920 
GMA02930 
GMA02940 
GMA02950 
GMA02960 
GMA02970 
GMA02980 
GMA02990 
GMA03000 
GMA03010 
GMA03020 
GMA03030 
GMA03040 
GMA03050 
GMA03060 
GMA03070 
GMA03080 
GMA03090 
GMA03100 
GMA03110 
GMA03120 
GMA03130 
GMA03140 
GMA03150 
GMA03160 
GMA03170 
GMA03180 
GMA03190 
GMA03200 
GMA03210 
GMA03220 
GMA03230 
GMA03240 
GMA03250 
GMA03260 
GMA03270 
GMA03280 


41 


165 

170 

175 

C 

C 

C 

C 


180 

185 

190 

C 

C 

C 

c 


195 

200 

205 

C 

C 

C 


***  START  OF  KALMAN  FILTER  *** 

M2=2*MODE 

***  COMPUTATION  OF  PK*AT  *** 

JK=2*SMODE-2 

DO  175  1=1, M2 
DO  170  J=1,M2 
JL=JK+J 
SUM=0. 0 

DO  165  K=1,M2 

JM=JK+K 

SUM  =SUM+PK(I,K)*A(JL,JM) 

CONTINUE 
TMP3(I,J)=SUM 
CONTINUE 
CONTINUE 

***  COMPUTATION  OF  A*(PK*AT)+  BQBT  =  PK1  *** 

DO  190  1=1, M2 
JL=JK+I 

DO  185  J=1,M2 
SUM=0.  0 

DO  180  K=1,M2 
JM=JK+K 

SUM=SUM+A(JL,JM)*TMP3(K,J) 
CONTINUE 
PK1( I , J)=SUM+BQBT( I , J) 
CONTINUE 
CONTINUE 


COMPUTE  PK1*CT 


»',>•-  -•--'- 


DO  205  1=1, M2 
DO  200  J=l,3 
SUM=0.  0 

DO  195  K=1,M2 

JM=JK+K 

SUM=SUM+PK1( I ,K)*C( J, JM) 

CONTINUE 
TMP1(I,J)=SUM 
CONTINUE 
CONTINUE 

***  COMPUTE  C*(PK1*CT)+RMN  *** 
DO  220  1=1,3 
DO  215  J=l,3 
SUM=0.  0 

DO  210  K=1,M2 
JM=JK+K 


GMA03290 
GMA03300 
GMA03310 
GMA03320 
GMA03330 
GMA03340 
GMA03350 
GMA03360 
GMA03370 
GMA03380 
GMA03390 
GMA03400 
GMA03410 
GMA03420 
GMA03430 
GMA03440 
GMA03450 
GMA03460 
GMA03470 
GMA03480 
GMA03490 
GMA03500 
GMA03510 
GMA03520 
GMA03530 
GMA03540 
GMA03550 
GMA03560 
GMA03570 
GMA03580 
GMA03590 
GMA03600 
GMA03610 
GMA03620 
GMA03630 
GMA03640 
GMA03650 
GMA03660 
GMA03670 
GMA03680 
GMA03690 
GMA03700 
GMA03710 
GMA03720 
GMA03730 
GMA03740 
GMA03750 
GMA03760 
GMA03770 
GMA03780 
GMA03790 
GMA03800 
GMA03810 
GMA03820 
GMA03830 
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210 

215 

220 

C 

C 

C 

C 

C 
C 
C 


235 

240 

245 

C 

C 

C 

C 


250 

255 

260 
C 


SUM=SUM+C(I,JM)*TMP1(K,J) 

CONTINUE 
TMP2(I,J)=SUM+RMN(I,J) 
CONTINUE 
CONTINUE 

***  COMPUTATION  OF  THE  INVERSE  OF  C*PK1*CT  +  R 


CALL  DLINRG  (  3,TMP2,3,TMP2,3) 

***  COMPUTE  CT*INV(C*PK1*CT+R) 

DO  245  1=1, M2 
JL=JK+I 

DO  240  J=l,3 
SUM=0.  0 

DO  235  K=l,3 

SUM=SUM+C(K,JL)*TMP2(K,J) 
CONTINUE 
TMP1(I,J)=SUM 
CONTINUE 
CONTINUE 

************* 

***  COMPUTE  PK1*C*INV(C*PK1*CT+R)  =  G  ***** 

DO  260  1=1, M2 
DO  255  J=l,3 
SUM=0.  0 

DO  250  K=1,M2 
SUM=SUM+PK 1 ( I , K ) *TMP 1  ( K , J ) 
CONTINUE 
G(I,J)=SUM 
CONTINUE 
CONTINUE 

N9=DABS((G(1,1)-H1)/G(1,1)) 
IF  (N9.GT.  ACHK)     THEN 
GO  TO  7377 
END  IF 

N9=DABS((G(1,3)-H2)/G(1,3)) 
IF  (N9.GT.  ACHK) THEN 
GO  TO  7377 
END  IF 

N9=DABS((G(2,1)-H3)/G(2,1)) 
IF  (N9.GT.  ACHK)     THEN 
GO  TO  7377 
END  IF 

N9=DABS((G(2,3)-H4)/G(2,3)) 
IF  (N9.GT.  ACHK)     THEN 
GO  TO  7377 
END  IF 

N9=DABS((G(3,3)-H5)/G(3,3)) 
IF  (N9.GT.  ACHK)     THEN 
GO  TO  7377 


GMA03840 

GMA03850 

GMA03860 

GMA03870 

GMA03880 

GMA03890 

GMA03900 

GMA03910 

GMA03920 

GMA03930 

GMA03940 

GMA03950 

GMA03960 

GMA03970 

GMA03980 

GMA03990 

GMA04000 

GMA04010 

GMA04020 

GMA04030 

GMA04040 

GMA04050 

GMA04060 

GMA04070 

GMA04080 

GMA04090 

GMA04100 

GMA04110 

GMA04120 

GMA04130 

GMA04140 

GMA04150 

GMA04160 

GMA04170 

GMA04180 

GMA04190 

GMA04200 

GMA04210 

GMA04220 

GMA04230 

GMA04240 

GMA04250 

GMA04260 

GMA04270 

GMA04280 

GMA04290 

GMA04300 

GMA04310 

GMA04320 

GMA04330 

GMA04340 

GMA04350 

GMA04360 

GMA04370 

GMA04380 

GMA04390 
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265 

270 

275 

C 

C 

C 

C 


280 

285 
290 
C 
C 


END  IF 

N9=DABS((G(M2,3)-H6)/G(M2,3)) 
IF  (N9.GT.  ACHK)     THEN 
GO  TO  7377 
END  IF 
GO  TO  400 


c 

c 

7377 

CONTINUE 

H1=G(1,1) 

H2=G(1,3) 

H3=G(2,1) 

H4=G(2,3) 

H5=G(3,3) 

H6=G(M2,3) 

IF  (TCHK.  LE.TIME)  THEN 

GO  TO  400 

END  IF 

IF  (CGN.  GE.PRT)   THEN 

WRITE  (6,*)  'TIME=  ',  TIME 

WRITE  (6,*)  *N9=  ' ,  N9 
CGN=0.  0 
END  IF 


1  SEC.  ' 


*  COMPUTE  IDENT 


G*C 


DO  275  1=1, M2 
DO  270  J=1,M2 
JL=JK+J 
SUM=0.  0 

DO  265  K=l,3 

SUM=SUM+G( I ,K)*C(K, JL) 

CONTINUE 
TMP3(I,J)=  IDENT(I,J)-SUM 
CONTINUE 
CONTINUE 

-'-  JL  - '  -  JL  JL  JL  JL  JL  JL  JL  JL  J  -  JL  JL  J-  JL  JL  JL  JL  JL 

***  COMPUTE  PK=  (INDENT  -  G*C)*PK1 

DO  290  1=1, M2 
DO  285  J=1,M2 
SUM=0.  0 

DO  280  K=1,M2 
SUM=SUM+TMP3(I,K)*PK1(K,J) 
CONTINUE 
PK(I,J)=SUM 
CONTINUE 
CONTINUE 


GMA04400 
GMA04410 
GMA04420 
GMA04430 
GMA04440 
GMA04450 
GMA04460 
GMA04470 
GMA04480 
GMA04490 
GMA04500 
GMA04510 
GMA04520 
GMA04530 
GMA04540 
GMA04550 
GMA04560 
GMA04570 
GMA04580 
GMA04590 
GMA04600 
GMA04610 
GMA04620 
GMA04630 
GMA04640 
GMA04650 
GMA04660 
GMA04670 
GMA04680 
GMA04690 
GMA04700 
GMA04710 
GMA04720 
GMA04730 
GMA04740 
GMA04750 
GMA04760 
GMA04770 
GMA04780 
GMA04790 
GMA04800 
GMA04810 
GMA04820 
GMA04830 
GMA04840 
GMA04850 
GMA04860 
GMA04870 
GMA04880 
GMA04890 
GMA04900 
GMA04910 
GMA04920 
GMA04930 
GMA04940 
GMA04950 
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c 
c 
c 
c 

c 

400 
C 


384 
5350 

C 
C 
C 


7153 

7154 

7155 

C 

C 

C 

C 

C 

C 
C 
C 


7157 

C 

C 

C 

599 

C 
C 
C 
C 
C 
C 
C 


END  OF  KALMAN  FILTER  PART  1  -  START  OF  PART  2  **** 


GO  TO  9991 

CONTINUE 

WRITE  (20,1008) 

WRITE  (20,*)  *TIME=  ' .TIME 

DO  384  1=1, M2 

WRITE  (20,5350)   G( I , 1) ,G( I ,2) ,G(I ,3) 

CONTINUE 

FORMAT  (*  ",5X,D15.8  ,5X,D15.8  ,5X,D15.8  ) 

WRITE  (20,*)  'N9=  ' ,N9 

***  COMPUTE  AGC  =  A  -  G*C 

M2=2*MODE 
JK=2*SMODE-2 

DO  7155  1=1, M2 
JL=JK+I 

DO  7154  J=1,M2 
JM=JK+J 
SUM=0.  0 

DO  7153  K=l,3 
SUM=SUM+G(I,K)*C(K,JM) 
CONTINUE 
AGC(I,J)=A(JL,JM)-SUM 
CONTINUE 
CONTINUE 


***  COMPUTE  THE  EIGENVALUES  OF  AGC 

CALL  DEVCRG  (M2,  AGC,  100,  EVAL,  EVEC,  100) 

****  PRINT  EVAL  (EIGENVALUE)  MATRIX 

DO  7157  1=1, M2 

WRITE  (20,*)  '1=  ',  I,  'EIG=  ',  EVAL(I) 

CONTINUE 


STOP 
END 


THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 


GMA04960 
GMA04970 
GMA04980 
GMA04990 
GMA05000 
GMA05010 
GMA05020 
GMA05030 
GMA05040 
GMA05050 
GMA05060 
GMA05070 
GMA05080 
GMA05090 
GMA05100 
GMA05110 
GMA05120 
GMA05130 
GMA05140 
GMA05150 
GMA05160 
GMA05170 
GMA05180 
GMA05190 
GMA05200 
GMA05210 
GMA05220 
GMA05230 
GMA05240 
GMA05250 
GMA05260 
GMA05270 
GMA05280 
GMA05290 
GMA05300 
GMA05310 
GMA05320 
GMA05330 
GMA05340 
GMA05350 
GMA05360 
GMA05370 
GMA05380 
GMA05390 
GMA05400 
GMA05410 
GMA05420 
GMA05430 
GMA05440 
GMA05450 
GMA05460 
GMA05470 
GMA05480 
GMA05490 
GMA05500 
GMA05510 
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OF  THE  100  MODES 

SUBROUTINE  STMTRX( EMODE , SMODE , T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+        R0WN1,R0WN2,R0WN3,BN) 

REAL*8  WN,GMA,PHI(2,2,100),GAMMA(2,100),EGT,T,COSW1T,SINW1T 

REAL*8  W1,D,A(200,200),B(200,3),C(6,200),BN(200,3) 

REAL  LAMA(100),UGVEX(684,100) 

INTEGER  SMODE ,R, EMODE, JJ,KK,R0WN1,R0WN2,R0WN3 


DO  600  I  =  1     ,100 

WN  =  DBLE(SQRT(LAMA(I))) 

GMA  =  D-WN/2.  0 

EGT  =  DEXP(-GMA*T) 

Wl  =  DSQRT((WN**2)-(GMA**2)) 

C0SW1T  =  DC0S(W1*T) 

SINW1T  =  DSIN(W1*T) 


. EQ. 0)THEN 

PHI(13 

1,1) 

= 

EGT*C' 

PHI(1. 

2,1) 

= 

T 

PHI  (2 

1,D 

= 

0 

PHI(2 

2,1) 

— 

EGT* 

•C0SW1T 

ELSE 


GAMMA(1,I)  =  0 
GAMMA(2,I)  =  0 


PHI(1,1,I)  =  EGT*(C0SW1T  +  (GMA*(W1**( -1) ) )*SINW1T) 

PHI(1,2,I)  =  (W1**(-1))*EGT*SINW1T 

PHI(2,1,I)  =  -(WN**2)*(W1**(-1))*EGT*SINW1T 

PHI(2,2,I)  =  EGT*(C0SW1T  -  (GMA*(W1**(-1)))*SINW1T) 


GAMMA(l,I)=(WN^(-2))*(l.D0-EG^COSWlT-EGT',f(GMA/Wl)"SINWlT) 


GMA05520 
GMA05530 
GMA05540 
GMA05550 
GMA05560 
GMA05570 
GMA05580 
GMA05590 
GMA05600 
GMA05610 
GMA05620 
GMA05630 
GMA05640 
GMA05650 
GMA05660 
GMA05670 
GMA05680 
GMA05690 
GMA05700 
GMA05710 
GMA05720 
GMA05730 
GMA05740 
GMA05750 
GMA05760 
GMA05770 
GMA05780 
GMA05790 
GMA05800 
GMA05810 
GMA05820 
GMA05830 
GMA05840 
GMA05850 
GMA05860 
GMA05870 
GMA05880 
GMA05890 
GMA05900 
GMA05910 
GMA05920 
GMA05930 
GMA05940 
GMA05950 
GMA05960 
GMA05970 
GMA05980 
GMA05990 
GMA06000 
GMA06010 
GMA06020 
GMA06030 
GMA06040 
GMA06050 
GMA06060 
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c 
c 
c 
c 

c 
c 
c 

600 

c 
c 
c 
c 


GAMMA(2,I)  =  (W1**(-1))*EGT*SINW1T 

ENDIF 
CONTINUE 

R  =  1 

DO  610  K  =  1    ,100 


A(R,R)  =  PHI(1,1,K) 
A(R,R+1)  =  PHI(1,2,K) 
A(R+1,R)  =  PHI(2,1,K) 
A(R+1,R+1)  =  PHI(2,2,K) 


'*   B  MATRIX  FOR  MULTIPLYING  CONTROL  TORQUES 

B(R,1)  =  GAMMA(1,K)*DBLE(UGVEX(412,K)) 
B(R,2)  =  GAMMA(1,K)*DBLE(UGVEX(413,K)) 
B(R,3)  =  GAMMA(1,K)*DBLE(UGVEX(414,K)) 
B(R+1,1)  =  GAMMA(2,K)*DBLE(UGVEX(412,K)) 
B(R+1,2)  =  GAMMA(2,K)*DBLE(UGVEX(413,K)) 
B(R+1,3)  =  GAMMA(2,K)*DBLE(UGVEX(414,K)) 


***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 


BN(R,1)=GAMMA(1,K)*DBLE(UGVEX(R0WN1,K)) 
BN(R,2)=GAMMA(l,K)*DBLE(UGVEX(ROWN2,K)) 
BN(R,3)=GAMMA(l,K)"DBLE(UGVEX(ROWN3,K)) 
BN(R+1,1)=GAMMA(2,K)*DBLE(UGVEX(R0WN1,K)) 
BN( R+l , 2 )=GAMMA( 2 , K) *DBLE( UGVEX( ROWN2 , K) ) 


GMA06070 
GMA06080 
GMA06090 
GMA06100 
GMA06110 
GMA06120 
GMA06130 
GMA06140 
GMA06150 
GMA06160 
GMA06170 
GMA06180 
GMA06190 
GMA06200 
GMA06210 
GMA06220 
GMA06230 
GMA06240 
GMA06250 
GMA06260 
GMA06270 
GMA06280 
GMA06290 
GMA06300 
GMA06310 
GMA06320 
GMA06330 
GMA06340 
GMA06350 
GMA06360 
GMA06370 
GMA06380 
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GMA06410 
GMA06420 
GMA06430 
GMA06440 
GMA06450 
GMA06460 
GMA06470 
GMA06480 
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GMA06510 
GMA06520 
GMA06530 
GMA06540 
GMA06550 
GMA06560 
GMA06570 
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GMA06600 
GMA06610 
GMA06620 
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c 
c 
c 
c 
c 
c 

610 

C 

C 

c 
c 
c 
c 
c 
c 
c 
c 
c 


BN(R+l,3)=GAMMA(2,K)*DBLE(UGVEX(ROWN3,K)) 


C 

640 

C 

C 

C 


R  =  R+2 
CONTINUE 


***  C  MATRIX  PRODUCTION  *** 


JJ=-1 

DO  640  1=1,100 

JJ=JJ+1 

KK=I+JJ 


C(1,KK)  =  DBLE(UGVEX(418,I)) 
C(2,KK)  =  DBLE(UGVEX(419,I)) 
C(3,KK)  =  DBLE(UGVEX(420,I)) 


KK=KK+1 

C(1,KK)=0.  0 
C(2,KK)=0.0 
C(3,KK)=0.0 

CONTINUE 


RETURN 
END 


GMA06630 
GMA06640 
GMA06650 
GMA06660 
GMA06670 
GMA06680 
GMA06690 
GMA06700 
GMA06710 
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GMA06730 
GMA06740 
GMA06750 
GMA06760 
GMA06770 
GMA06780 
GMA06790 
GMA06800 
GMA06810 
GMA06820 
GMA06830 
GMA06840 
GMA06850 
GMA06860 
GMA06870 
GMA06880 
GMA06890 
GMA06900 
GMA06910 
GMA06920 
GMA06930 
GMA06940 
GMA06950 
GMA06960 
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c 
c 

c 
c 

c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


APPENDIX      B.  KALMAN  OBSERVER  AND  PLANT  SIMULATION 

**********  AAA  *  *************  A  A  A  A  A  A  ***********  A  A********  A  'A  A  A  A 
*****  SIMRUN  ***** 

*****  ADAPTED  TO  READ  KALMAN  FILETER  G  MATRICE  ***** 

*****  then  RUN  ALL  N   MODES  OF  THE  PLANT  WHILE  ***** 

*****  USING  A  KALMAN  FILTER  TO  OBSERVE  M  ***** 

*****  NUMBER  OF  STATES  ***** 

*********************************************************** 


*********************************************************** 

*****  VARIABLE  DECLARATIONS  ***** 

********************************************  *************** 

EXTERNAL  STMTRX,EXCMS 

CHARACTERS  NAM 

CHARACTER* 1  AGAIN ,CORECT,RAGAIN 

INTEGER  ROWNl,ROWN2,ROWN3, COUNT, NODE, MODE, KQ,EM0DE,SM0DE,R2M,C2M 

INTEGER  CT,CF,KADJ,CFADJ,L00P,PRNT,JJ,JK,N1,JR,KR,MR,ISEED,M2 

INTEGER  ITYPE(200),  IPVT(IOO),  NS ,  NF,  SN,  FN 

INTEGER  JL,J1,JM  ,  JP,  JQ,  KA,  KB,  KC,  KD,  KE,  KF,  KG 


REAL  LAMA(IOO),  UGVEX(684, 100) ,RNODE,RMODE,MIN 

REAL*8  PHI(2,2,100),GAMMA(2,100),EGT,GMA,WN,W1,X1T,X2T,TIME 

REAL*8  A(200,200),B(200,3),F(3,  50) , IMPLSE , ENERGY 

REAL*8  C0SW1T,SINW1T,X(200) 

REAL*8  TCX , TCY , TCZ , DAMP , SAMPT , PI , S AMPTM , SUM 1 , SUM2 , SUM3 , SUMC 

REAL*8  C(3,200),  IDENT(  50,  50),  RMN(3,3),  QPN(3,3) 

REAL*8  Y(3)   ,  BN(200,3) 

REAL*8  PNVARX,  PNVARY,  PNVARZ 

REAL*8  MNVARX,  MNVARY,  MNVARZ 

REAL*8  SUM,  RNDM(6),  RND1,  RND2 

REAL*8  XH(  50)   ,BQBT(  50,  50) 

REAL* 8  SF1 

REAL*8  TMP1C  50,3),  TMP2(3,3),  TMP3(  50,  50) 

REAL* 8  G(  50,3) 

REAL*8  XH1(  50)   ,DY(3)   ,  ES,ED,ESUM,CGN,PRT 

REAL*8  WT  ,  WD(3),  BNVD(200) 

REAL*8  AX(200)  ,  V(3),  SF  ,  TO,  CTT,  ESS 

REAL*8  CTG,  XDEL,  E2(100),  XDEL1,  ERS,  PRT1,  E3(100),  XS(IOO) 

*********************************************************** 
*****  VARIABLE  DEFINITIONS  ***** 

*********************************************************** 

STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 
LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 
UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 
PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 


SIM00010 
SIM00020 
SIM00030 
SIM00040 
SIM00050 
SIM00060 
SIM00070 
SIM00080 
SIM00090 
SIM00100 
SIM00110 
SIM00120 
SIM00130 
SIM00140 
SIM00150 
SIM00160 
SIM00170 
SIM00180 
SIM00190 
SIM00200 
SIM00210 
SIM00220 
SIM00230 
SIM00240 
SIM00250 
SIM00260 
SIM00270 
SIM00280 
SIM00290 
SIM00300 
SIM00310 
SIM00320 
SIM00330 
SIM00340 
SIM00350 
SIM00360 
SIM00370 
SIM00380 
SIM00390 
SIM00400 
SIM00410 
SIM00420 
SIM00430 
SIM00440 
SIM00450 
SIM00460 
SIM00470 
SIM00480 
SIM00490 
SIM00500 
SIM00510 
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GAMMA  =  INPUT  TRANSITION  MATRIX 

A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 

B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 

TCX,  TCY,  TCZ  =  CONTROL  TORQUE  VALUES 

ENERGY  =  TOTAL  SYSTEM  ENERGY 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 
ROWN1  =  X-SLOPE  LOCATION 
ROWN2  =  Y-SLOPE  LOCATION 
ROWN3  =  Z -SLOPE  LOCATION 
C  =  OUTPUT  MATRIX  FOR  Y 
IDENT  =  IDENTITY  MATRIX 

RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 
QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 
PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 
PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 
PNVARZ  =  PLANT  NOISE  Z- SLOPE  VARIANCE 
MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 
MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 
MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE 
I SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 
XKAL  =  X  MATRIX 
Y  =  OUTPUT  MATRIX 
RNDM  =  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 

IN  PLANT  FORCES 
BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 
TNX,TNY,TNZ=  NOISE  TORQUES  X,Y,Z  SLOPES 
M2=2*MODE 


•sV'jW-'jWoV'sYsV^V'jV 


SAMPLE  OF  SPACE  EXEC  FILE 


^V'fc'ft^'sV'jVsV'ft'jWoV'sY'sV'sViV'jY-'oY 


THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 


FORTVS  SPACE 

SPACE 

LOAD  SPACE  (START 


(COMPILES  PROGRAM) 

(EXECUTES  EXEC  FILE) 

(LOADS  AND  EXECUTES  PROGRAM) 


SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 

FI  4  DISK  THESIS  INPUT  B  (PERM 

FI  8  DISK  UTILITY  DATA   (RECFM  VS  BLOCK  133  PERM 

FI  11  DISK  CNTRL  OUTPUT  (RECFM  F  BLOCK  80  LRECL  80  PERM 

FI  13  DISK  GAMMA  OUTPUT  (RECFM  VS  BLOCK  133  PERM 

FI  14  DISK  MODE  OUTPUT   (RECFM  F  BLOCK  80  LRECL  80  PERM 
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5 

C 

1001 

1002 

1008 

C 

500 

C 

C 

C 


700 

C 

C 

C 

C 


FI    16   DISK  COST  OUTPUT      (RECFM  F  BLOCK   80   LRECL   80   PERM 
FI    17   DISK   PRT     OUTPUT      (RECFM  F   BLOCK   80   LRECL  80   PERM 
FI    18  DISK  ERROR  DATA        (RECFM  F  BLOCK  80  LRECL  80  PERM 
FI    19   DISK  END  FILE        (RECFM  F  BLOCK  80   LRECL  80  PERM 
FI   20  DISK  GMAT  FILE   (RECFM  F  BLOCK  80  LRECL  80  PERM 

**************************************************************** 

PARAMETER   (JR=5243,   KR=5397,    MR=262139) 


MIN  =1.00 

WT=1.  0D00 

PI  =  4.  0D0  *  ATAN(1.  0D0) 

*********************************************************** 

*****  READ  LAMA  AND  UGVEX  MATRICIES  ***** 

*********************************************************** 

CALL  EXCMS    ('CLRSCRN') 

WRITE(6,1008) 

WRITE (6,*)    ' 

WRITE(6,*)    '     * 

THIS   SECTION  READS  THE   LAMA  VECTOR  AND  THE  UGVEX 
MATRIX  AND  STORES  THEM   IN  MEMORY  FOR  FURTHER  RECALL  OF 
DESIRED  LOCATION  DATA. 


READING  LAMA  AND  UGVEX  MATRICIES* 


READ(4,1001)   NAM 

RE AD( 4, 1002) (LAMA( I), 1=1, 100) 

READ(4,1001)    NAM 

DO  5   J  =   1,100 

READ(4,1002)(UGVEX(I,J),I=1,684) 
CONTINUE 

F0RMAT(1X,A6) 
F0RMAT(1X,8E15. 8) 
FORMAT   (1X,////) 

CALL  EXCMS   (' CLRSCRN') 

************  STARTING  MODE  NUMBER 

**   SMODE    7   TO    100    (INTEGER)    **** 
SMODE=   7 

WRITE   (16,700)    SMODE 

FORMAT   ('    ',' STARTING  MODE  NUMBER:       ',12) 

***************  NUMBER  OF  MODES  TO  SCAN 

**  MODE    1  TO   93    (INTEGER) 

MODE=20 

EMODE  =  SMODE  +  MODE   -    1 


•.'-  -.<■• »'-  •.'-  JU  »'-  «J-  *J-  •»'-  »'-  ".'p  »'•  »'-  -'- 


************* 
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701 
C 
C 
C 


702 

C 

C 


899 

C 

C 


900 
C 

703 
C 
C 
C 


704 
C 

c 
c 
c 


WRITE   (16,701)   MODE 

FORMAT  ('    ',' NUMBER  OF  MODES   SCANNED:       ',12) 

************  NOISE   INPUT  POSITION  ***************** 

**  NODE    1  TO   114   (INTEGER)    (IF  0  THEN  NO  NOISE   INPUT) 
NODE=  8 

WRITE   (16,702)   NODE 

FORMAT  ('    *,'    NOISE  NODE  LOCATION:         ',15) 

***********  START  AND  STOP  FOR  PLANT 

SN=7 

FN=20 

NS=SN*2-1 

NF=SN*2+2*FN-2 

WRITE   (16,899)    SN,FN 

FORMAT  ('    V  PLANT  --   SN=   ',15,'      FN=   ',15) 

*************  SAMPLING  TIME  ************** 

**  SAMPT  MUST  BE   LESS  THAN  OR  EQUAL  TO   SAMPTM  ** 
SAMPT  =  0.  05 

SAMPTM  =   ((2.0D0*PI)/SQRT(LAMA(EMODE)))/1.0D01 

IF   (SAMPT. GE. SAMPTM)   THEN 
SAMPT=SAMPTM 

ENDIF 

WRITE   (16,900)   MIN 

FORMAT   ('    ' ,2X,'MIN:       ' ,F8. 3) 

WRITE   (16,703)    SAMPT,    SAMPTM 

FORMAT   ('    ', 'SAMPLING  TIME:       ' ,D12. 4,2X, ' SAMPTM=    ' ,D15. 8) 


•J—  *f «  -'-  -'  .  »*»  -*-  -.'«-'.•  ..'.»  »**  aj*  »**  «.*..  -. '.  «.'.. 

J.  J. 


DAMPING  FACTOR 
DAMP   0.0  TO   1.0   (REAL*8) 
DAMP=.  01 


■  -  ■-  »**  «.'■  -.'»  *'• *J*  «.•»  ■Jtp  J»  »'*  •.*-  J 


WRITE   (16,704)   DAMP 

FORMAT  ( '    ' , 'DAMPING  FACTOR:       ' ,D12. 4) 


***  PLANT  NOISE  VARIANCE  *** 

**  PNVARX,    PNVARY,    PNVARZ   GT  0.  0 

SF1=2.5D06 

SF=1.  ODOO 

PNVARX=1.  0D00*SF1 
PNVARY=1. 0D00*SF1 
PNVARZ=1. 0D00*SF1 


***  MEASUREMENT  NOISE  VARIANCE  *** 
**  MNVARX,    MNVARY,    MNVARZ  GT  0.  0 
MNVARX=1. OD-03  *SF 
MNVARY=1.  OD-03  *SF 
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c 
c 

711 
712 
713 
714 
715 

C 

510 


48 
C 


54 

C 

C 


58 
60 
C 


MNVARZ=1.  OD-03  *SF 


WRITE   (16,711) 

FORMATC    ','  PLANT  NOISE  VARIANCE:       ') 

WRITE   (16,712) 

FORMATC    '  .ex/PNVARX'jISX/PNVARY'  ,  13X, 'PNVARZ' ) 

WRITE   (16,713)    PNVARX,    PNVARY,    PNVARZ 

FORMATC '    ' ,2X,E15. 8,2X,E15. 8,2X,E15. 8) 

WRITE( 16,714) 

FORMAT( '    ' , ' MEASUREMENT  NOISE: ' ) 

WRITE( 16,715) 

FORMATC    '  ^X/MNVARX*  , 13X, 'MNVARY*  , 13X, ' MNVARZ ' ) 

WRITE( 16,713)   MNVARX, MNVARY, MNVARZ 


CALL  EXCMS   ('CLRSCRN') 
WRITE   (6,1008) 
WRITE   (6,*)    ' 


PROGRAM  RUNNING' 


J -  . '-  ■.'-  .J-  ^-  ^.  -' ;  J-  J*.  JL  . I - 


NOISE   INPUT  LOCATION 


.' -  J -  -'-  - •-  y .  J-  J-  -'  -  .'.  .< .  J  -  -'  - 


R0WN3  =  NODE*6 
ROWN2  =  (NODE*6)    -    1 
ROWN1   =   (NODE*6)    -    2 
COUNT  =  0 


. . '  -  -  •  -  - '- ,. '  -  J  .  ~'  -  ~.  JLi .  •  -  y  - 


INITIALIZE  MATRICIES 


..'..  «.'..  «j.  .'»  «'»  »*»  •.'■•  *.**  »fj«  -'»  j«  . '» >J—  fc'p  J»  J* 


DO  48  K=l,50 

IDENT(K,K)=1.0 

CONTINUE 

DO  54  K  =   1,    200 
X(K)   =  0.  0 

CONTINUE 


WRITE(6,1008) 


WRITE   (6,*)        INITIALIZE  RMN  AND  QPN  MATRICES 
***   INITIALIZE  RMN  AND  QPN  MATRICES  *** 

DO  60   1=1,3 

DO  58  J=l,3 

RMN(I,J)=0.0 

QPN(I,J)=0.0 

CONTINUE 
CONTINUE 

RMN(1,1)=MNVARX**2 
RMN(2,2)=MNVARY**2 
RMN(3,3)=MNVARZ**2 
QPN(1,1)=PNVARX**2 
QPN(2,2)=PNVARY**2 
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QPN( 3 , 3)=PNVARZ**2. 0 


WRITE(6,1008) 

WRITE (6,*)      '    ENTER  STMTRX  ' 

****************************************************^^ 

*****  BEGIN  MAIN  PROGRAM  ***** 

*********************************************************** 

CALL  STMTRX ( EMODE , SMODE , SAMPT , DAMP , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
h       ROWNl,ROWN2,ROWN3,BN) 


WRITE   (16,1008) 

DO   11000   1=1,200 
DO   10900  J=l,3 
C(J,I)=  C(    J,I)*SF 
10900  CONTINUE 

11000   CONTINUE 
C 

C  ***  PRE -LOOP  PORTION  OF  KALMAN  FILTER 

C 

M2=2*M0DE 

JP=2*SMODE-l 

JQ=2*EM0DE 

DO   90   1=1,50 
XH(I)=0.  0 

CONTINUE 

DO   9971    1=1, M2 

READ   (20,*)   G(I,1),    G(I,2),    G(I,3) 

CONTINUE 


90 
C 


9971 

C 

C 


384 


9771 


WRITE   (14,1008) 
DO  384   1=1, M2 
WRITE   (14,5350) 
CONTINUE 


G(I,1),G(I,2),G(I,3) 


5350     FORMAT   ('    '  ,2X,D15. 8,2X,D15.  8,2X,D15.  8) 


********************************************************** 
*****  this   SECTION  COMPUTES  THE   STATE  UPDATE  ***** 

-'-  »'»  •.'-  •.'»  «' »  -'.  •.'..  J*  ..'..  «1*  * '-  -I-  .J*  «.i  *  »'»  -.*..  »*.  «.t  _  ^.t^  ^1^  _f „  _•„  _!„  _t ,,  _t_  _*_  jj  _f _  ^t _  ^; j,  _ij  ,j,»  _.'..  _'—  _'a  «J_,  _.'-  ^f ,,  fct ,  ..t .  j_  J —  j^  _i—  fcr j  ..f^  ^T„  «.!,.  ^tj  «J—  ^t,.  J„  _t_  ..',,  J-  ^f,  _!„  ^1+ 

DO  9771   1=1,100 
E2(I)=0.  0 

E3(I)=0.0 
XS(I)=0.0 

CONTINUE 
ESS  =0.0 
COUNT  =  0 
ENERGY  =  0.  0D0 
TIME  =  0.0 
CGN=0.  0 
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CTG=0.  0 

*****  SETS  LOOP  FOR  THE  ITERATIONS  NECESSARY  TO  OBSERVE  ***** 
*****  THE  SYSTEM  FOR  THE  NUMBER  OF  MINUTES  SPECIFIED  ***** 
WRITE  (6,1008) 


101 
C 

C 
C 
C 
C 
C 
C 
C 


5010 

5015 
C 
C 
C 


WRITE  (6,*) 


START  STATE  UPDATE 


LOOP  =  INT((MIN*60.0)/SAMPT) 
PRT=  (DBLE(LOOP))/30.  0 
CTT=0.  0 

DO  400  L  =0,  LOOP 

TIME  =  DBLE(L)*SAMPT 

IF(L.  EQ.  0)THEN 

IMPLSE  =(1.  0D06*SF1)/(DSQRT(SAMPT)) 
ELSE 

IMPLSE  =  0.  0D0 
END  IF 

TO=0.  0 

*****  RANDOM  NUMBER  GENERATOR  ***** 

DO  101  1=1,6 

I SEED=MOD ( I SEED* JR+KR , MR ) 

RND1=(DBLE(ISEED)+0.5D00)/DBLE(MR) 

I SEED=MOD( I SEED* JR+KR , MR ) 

RND2=(DBLE(ISEED)+0.5D00)/DBLE(MR) 

RNDM(I)=DSQRT(-2. 0*DLOG(RNDl))*DCOS(6. 2831853D00*RND2) 

CONTINUE 

******************* 

CTT=CTT+1.  0 

****  START  OF  STATE  UPDATE  *** 

***  COMPUTE  AX°200  =  A0 200  X  200  *  X°200 


***  COMPUTE  AX  =  A*X 

JK=SMODE*2-2 

JP=JK+1 

JQ=2*EMODE 


DO  5015  I=NS,NF 
SUM=0.  0 

DO  5010  K=NS,NF 
SUM=SUM+A(I,K)*X(K) 
CONTINUE 
AX(I)=SUM 
CONTINUE 

***  COMPUTE  WD°3 

WD( 1)=PNVARX*RNDM( l)*TO+IMPLSE 
WD ( 2 ) =PNV AR Y*RNDM ( 2 ) *TO 
WD( 3)=PNVARZ*RNDM( 3)*TO 
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5030 

5035 
C 
C 
C 


C 
C 

C 

5040 

C 

C 

C 


5045 

5050 

C 

C 

C 

C 

C 

C 
C 
C 
C 


295 

300 
C 


***  COMPUTE  BNWD°200  =BN°200  X  3  *  WD°3 

DO  5035  I=NS,NF 
SUM=0.  0 

DO  5030  K=l,3 
SUM=SUM+BN(I,K)*WD(K) 
CONTINUE 
BNWD(I)=SUM 
CONTINUE 

***  COMPUTE  X°200  =AX°200  +  BNWD°200 

DO  5040  I=NS,NF 

X(I)=  AX(I)  +  BNWD(I) 

IF  (DABS(X(I)).LT.  1.  OD-60)  THEN 

X(I)=1.  OD-60 
END  IF 


CONTINUE 

***  COMPUTE  V°3 

V(1)=MNVARX*RNDM(4) 
V(2)=MNVARY*RNDM(5) 
V(  3  )  =MNVARZ*RNDM(  6 ) 

***  COMPUTE  Y°3  =  C°3  X  200  *  X°200  +  V°3 

DO  5050  1=1,3 
SUM=0.  0 

DO  5045  K=NS,NF 
SUM=SUM+C(I,K)*X(K) 
CONTINUE 
Y(I)=SUM+V(I) 
CONTINUE 

***  START  OF  KALMAN  FILTER  *** 
M2=2*M0DE 

***  COMPUTE  XH1  =  A*XH 

DO  300  I=JP,JQ 
SUM=0.  0 

DO  295  K=JP,JQ 

SUM=SUM+A(I,K)  *  XH(K) 

CONTINUE 
XH1(I)=SUM 
CONTINUE 
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310 

315 

C 

C 

C 

C 

C 


320 


C 

325 

C 

C 

C 

c 
c 
c 


340 
C 


C 

c 
c 

2100 


***  COMPUTE  DY  =  Y  -  C*XH1 

DO  315  1=1,3 
SUM=0.  0 

DO  310  K=JP,JQ 

SUM=SUM+C(I,K)*XH1(K) 

CONTINUE 
DY(I)=Y(I)-SUM 
CONTINUE 

***   COMPUTE  XH  =  XH1  +  G*DY 

DO  325  1=1, M2 
J1=JP-1+I 
SUM=0.  0 

DO  320  K=l,3 

SUM=SUM+G(I,K)*DY(K) 

CONTINUE 
XH(J1)=XH1(J1)+SUM 
IF  (DABS(XH(J1)).LT.  1.0D-60)  THEN 

XH(J1)  =  1.  0*D-60 
END  IF 

CONTINUE 


*****   END  OF  KALMAN  ROUTINES  ****** 

***  COMPUTATION  OF  ESUM  *** 

DO  340  I=JP,JQ 

XDEL=  X(I)-XH(I) 

XDEL1=XDEL*XDEL*SAMPT 

E2(I)=E2(I)+XDEL1 

XS(I)=XS(I)+X(I)*X(I)*SAMPT 

E3(I)=E2(I)/XS(I) 

CONTINUE 

CGN=CGN+1.  0 

IF  (CTT.  EQ.  l.O.OR.  CGN.  GT.  PRT)  THEN 

WRITE  (6,*)  'TIME=  ',  TIME,  '  SEC.' 

WRITE  (17,1008) 
WRITE  (16,1008) 
WRITE  (16,2100)  TIME 

WRITE  (17,2100)  TIME 

FORMATC  ','TIME=   \F9.3) 

DO  380  I=JP  ,  JQ 

WRITE  (16,4500)  I,X(I)  ,1  ,XH(I) 
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380 

C 

C 

C 

C 


4500 

4530 

C 

400 

C 

c 


401 
C 

c 
c 
c 

599 

C 
C 
C 
C 
C 
C 
C 


WRITE  (17,4530)  I,E2(I)   ,E3(I)    ,  XS(I) 

CONTINUE 


CGN=0.  0 
END  IF 


FORMAT  ('  ','  X(',I3,')=  ' ,D15. 8,2X, !XH( ' ,13, ' )=  ' ,D15. 8) 
FORMAT  ('  ' ,5X,I5,5X,3  D15.8) 

CONTINUE 


DO  401  I=JP,JQ 

WRITE  (19,4530)  I,  E2(I)  ,E3(I),  XS(I) 

CONTINUE 


STOP 
END 


THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 

OF  THE  100  MODES 

?vivyc-Wcy-yrycy-yoV}Vyr}vyoV-vycywry«v<V}vyov^^ 

SUBROUTINE  STMTRX( EMODE , SMODE , T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+        R0WN1,R0WN2,R0WN3,BN) 

REAL-8  WN,GMA,PHI(2,2,100),GAMMA(2,100),EGT,T,COSW1T,SINW1T 

REAL*8  W1,D,A(2005200),B(200,3),C(3,200),BN(200,3) 

REAL  LAMA(100),UGVEX(684,100) 

INTEGER  SMODE , R , EMODE , J J , KK , ROWN 1 , ROWN2 , ROWN3 


DO  600  I  =  1     ,100 

WN  =  DBLE(SQRT(LAMA(I))) 

GMA  =  D--WN/2.  0 

EGT  =  DEXP( -GMA*T) 

Wl  =  DSQRT((WN**2)-(GMA**2)) 

COSW1T  =  DC0S(W1*T) 

SINW1T  =  DSIN(W1*T) 


IF(WN.  EQ.  0)THEN 

PHI(1,1,I) 

=  EGT*C0SW1T 

PHI(1,2,I) 

=  T 

PHI(2,1,I) 

=  0 

PHI(2,2,I) 

=  EGT*C0SW1T 

SIM04930 
SIM04940 
SIM04950 
SIM04960 
SIM04970 
SIM04980 
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SIM05000 
SIM05010 
SIM05020 
SIM05030 
SIM05040 
SIM05050 
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SIM05070 
SIM05080 
SIM05090 
SIM05100 
SIM05110 
SIM05120 
SIM05130 
SIM05140 
SIM05150 
SIM05160 
SIM05170 
SIM05180 
SIM05190 
SIM05200 
SIM05210 
SIM05220 
SIM05230 
SIM05240 
SIM05250 
SIM05260 
SIM05270 
SIM05280 
SIM05290 
SIM05300 
SIM05310 
SIM05320 
SIM05330 
SIM05340 
SIM05350 
SIM05360 
SIM05370 
SIM05380 
SIM05390 
SIM05400 
SIM05410 
SIM05420 
SIM05430 
SIM05440 
SIM05450 
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SIM05480 
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c 
c 
c 
c 

c 
c 
c 

600 

c 
c 


ELSE 


GAMMA(1,I)  =  0 
GAMMA(2,I)  =  0 


PHI (1,1, I)  =  EGT*(C0SW1T  +  (GMA*(W1**(-1)))*SINW1T) 

PHI(1,2,I)  =  (W1**(-1))*EGT*SINW1T 

PHI(2,1,I)  =  -(WN**2)*(W1**(-1))*EGT*SINW1T 

PHI(2,2,I)  =  EGT*(C0SW1T  -  (GMA*(W1**(  -1)))*SINW1T) 


GAMMA(  1 , I)=(WN**(  -2)  )*(  1.  0D0-EGT*COSWlT 
GAMMA(2,I)  =  (W1**(-1))*EGT*SINW1T 


ENDIF 

CONTINUE 

R  =  1 

DO  610  K  =  1     ,100 


■EGT*(GMA/W1)*SINW1T) 


A(R,R)  =  PHI(1,1,K) 
A(R,R+1)  =  PHI(1,2,K) 
A(R+1,R)  =  PHI(2,1,K) 
A(R+1,R+1)  =  PHI(2,2,K) 


***  B  MATRIX  FOR  MULTIPLYING  CONTROL  TORQUES 

B(R,1)  =  GAMMA(1,K)*DBLE(UGVEX(412,K)) 
B(R,2)  =  GAMMA(1,K)*DBLE(UGVEX(413,K)) 
B(R,3)  =  GAMMA(1,K)*DBLE(UGVEX(414,K)) 
B(R+1,1)  =  GAMMA(2,K)*DBLE(UGVEX(412,K)) 


SIM05490 
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SIM05630 
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SIM05650 
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SIM05670 
SIM05680 
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SIM05730 
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SIM05780 
SIM05790 
SIM05800 
SIM05810 
SIM05820 
SIM05830 
SIM05840 
SIM05850 
SIM05860 
SIM05870 
SIM05880 
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B(R+1,2)  =  GAMMA(2,K)*DBLE(UGVEX(413,K)) 
B(R+1,3)  =  GAMMA(2,K)*DBLE(UGVEX(414,K)) 


C 
C 

c 
c 
c 
c 

610 

C 

C 

C 

C 

C 

c 


***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 


BN(R,1)=GAMMA(1,K)*DBLE(UGVEX(R0WN1,K)) 

BN(R,2)=GAMMA(lJK)*DBLE(UGVEX(ROWN2,K)) 

BN(R,3)=GAMMA(l,K)*DBLE(UGVEX(ROWN3,K)) 

BN(R+1,1)=GAMMA(2,K)*DBLE(UGVEX(R0WN1,K)) 

BN(R+l,2)=GAMMA(2,K)*DBLE(UGVEX(ROWN2,K)) 

BN( R+l , 3)=GAMMA( 2 ,K)*DBLE( UGVEX(ROWN3 ,K) ) 


C 

640 

C 

C 


R  =  R+2 

CONTINUE 

***  C  MATRIX  PRODUCTION  **' 


JJ=-1 

DO  640  1=1,100 

JJ=JJ+1 

KK=I        +JJ 


C(1,KK)  =  DBLE(UGVEX(418,I)) 
C(2,KK)  =  DBLE(UGVEX(419,I)) 
C(3,KK)  =  DBLE(UGVEX(420,I)) 


KK=KK+1 

C(1,KK)=0.0 
C(2,KK)=0.0 
C(3,KK)=0.0 

CONTINUE 
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SIM06180 
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SIM06300 
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p  SIM06610 

PFTTTDM  SIM06620 

J?™^  SIM06630 

hND  SIM06640 


61 


APPENDIX      C.  PROGRAM  TO  ESTIMATE  NOISE  IN  KALMAN  FILTER 


c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


FROM  UNOBSERVED  MODES 


*********************************************************** 

*****  SPAC  24  ***** 

*****  ADAPTED  TO  RUN  N  MODES  OF  THE  PLANT  AND  ***** 
*****  COMPUTE  THE  NOISE  IN  THE  KALMAN  FILTER  ***** 
*****  FRom  THE  UNOBSERVED  MODES  ***** 

*********************************************************** 


*****  VARIABLE  DECLARATIONS  ***** 

*********************************************************** 

EXTERNAL  STMTRX,EXCMS 

CHARACTERS  NAM 

CHARACTER*1  AGAIN, CORECT,RAGAIN 

INTEGER  ROWN 1 , ROWN2 , R0WN3 , COUNT , NODE , MODE , KQ , EMODE , SMODE , R2M , C2M 

INTEGER  CT,CF,K^DJ,CFADJ,L00P,PRNT,JJ,JK,N1,JR,KR,MR,ISEED,M2 

INTEGER  NO,  NS,  NF,  SN,  FN 

INTEGER  JL,J1,JM  ,  JP,  JQ,  KA,  KB,  KC,  KD,  KE,  KF,  KG 


REAL  LAMA(IOO),  UGVEX(684, 100) ,RN0DE,RM0DE,MIN 

REAL*8  PHI(2,2,100),GAMMA(2,100),EGT,GMA,WN,W1,X1T,X2T,TIME 

REAL*8  A(200,200),B(200,3),F(3,  50) , IMPLSE, ENERGY 

REAL- 8  COSW1T,SINW1T,X(200) 

REAL*8  DAMP , SAMPT , PI , SAMPTM , SUM1 , SUM2 , SUM3 , SUMC 

REAL-8  C(9,200),  RMN(3,3),  QPN(3,3) 

REAL*8  BN( 200,3) 

REAL*8  PNVARX,  PNVARY,  PNVARZ 

REAL*8  MNVARX,  MNVARY,  MNVARZ 

REAL*8  SUM,  RNDM(6),  RND1,  RND2 

REAL* 8  ES,ED,ESUM,CGN,PRT 

REAL*8  WT  ,  WD(3),   BNWD(200),  EX1(9) 

REAL*8  EX(9),AX(200)  ,  SF  ,  TO,  CTT,  ESS 

REAL*8  CTG,  XDEL,   XDEL1,  ERS,  PRT1 

REAL*8  SF1 

*********************************************************** 

*****  VARIABLE  DEFINITIONS  ***** 

*********************************************************** 

STMTRX  =  SUBROUTINE  EXTABLISHES  STATE  TRANSITION  MATRICIES 

LAMA  =  VECTOR  OF  THE  SQUARE  OF  THE  NATURAL  FREQUENCIES 

UGVEX  =  MODE  POSITONS  AND  SLOPES  OF  THE  NODAL  POINTS 

PHI  =  STATE  TRANSITION  MATRICIES  FOR  EACH  MODE 

GAMMA  =  INPUT  TRANSITION  MATRIX 

A  =  DIAGONAL  MATRIX  CONSISTING  OF  PHI 
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SPA00170 
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SPA00220 
SPA00230 
SPA00240 
SPA00250 
SPA00260 
SPA00270 
SPA00280 
SPA00290 
SPA00300 
SPA00310 
SPA00320 
SPA00330 
SPA00340 
SPA00350 
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SPA00370 
SPA00380 
SPA00390 
SPA00400 
SPA00410 
SPA00420 
SPA00430 
SPA00440 
SPA00450 
SPA00460 
SPA00470 
SPA00480 
SPA00490 
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c 

c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


B  =  INPUT  MATRIX  OF  GAMMA  AND  CONTROL  SLOPES 

DAMP  =  DAMPING  FACTOR 

SAMPT  =  SAMPLING  TIME 

IMPLSE  =  IMPULSE  INPUT  FUNCTION 

MIN  =  NUMBER  OF  MINUTES  SYSTEM  WILL  BE  OBSERVED 

SMODE  =  NUMBER  OF  STARTING  MODE  (INT) 

MODE  =  NUMBER  OF  MODES  (INT) 

EMODE  =  NUMBER  OF  THE  LAST  MODE  (INT) 

NODE  =  NUMBER  OF  THE  NOISE  INPUT  MODE  (INT) 

***  NOISE  SLOPE  LOCATIONS  IN  DATA  MATRIX  *** 
ROWN1  =  X-SLOPE  LOCATION 
ROWN2  =  Y- SLOPE  LOCATION 
ROWN3  =  Z- SLOPE  LOCATION 
C  =  OUTPUT  MATRIX  FOR  Y 
IDENT  =  IDENTITY  MATRIX 

RMN  =  MEASUREMENT  NOISE  COVARIANCE  MATRIX 
QPN  =  PLANT  NOISE  COVARIANCE  MATRIX 
PNVARX  =  PLANT  NOISE  X-SLOPE  VARIANCE 
PNVARY  =  PLANT  NOISE  Y-SLOPE  VARIANCE 
PNVARZ  =  PLANT  NOISE  Z-SLOPE  VARIANCE 
MNVARX  =  MEASUREMENT  NOISE  X-SLOPE  VARIANCE 
MNVARY  =  MEASUREMENT  NOISE  Y-SLOPE  VARIANCE 
MNVARZ  =  MEASUREMENT  NOISE  Z-SLOPE  VARIANCE  ■ 
I  SEED  =  INITIALIZATION  FOR  RANDOM  NUMBER  GENERATOR 
RNDM  =  RANDOM  NUMBERS  USED  FOR  WHITE  NOISE  IN  MEASUREMENTS  AND 

IN  PLANT  FORCES 
BN  =  B  MATRIX  TO  MULTIPLY  NOISE  DISTURBANCES 


Vr'j'r'jV'sV'jV'^r'jVVfVf'jV 


SAMPLE  OF  SPACE  EXEC  FILE 


^Vc^V'sWcyry-'jWc'jWoV'sV'sWt'sWr 


THIS  FILE  MUST  BEGIN  IN  COLUMN  1  AND  RUN  WITH  THE  FOLLOWING 
SEQUENCE  FOR  THE  INITIAL  RUN  OF  THE  PROGRAM: 


FORTVS  SPACE 

SPACE 

LOAD  SPACE  (START 


(COMPILES  PROGRAM) 

(EXECUTES  EXEC  FILE) 

(LOADS  AND  EXECUTES  PROGRAM) 


SUBSEQUENT  PROGRAM  RUNS  CAN  ELIMINATE  "FORTVS  SPACE"  IF  NO 
CHANGES  HAVE  BEEN  MADE  TO  THE  PROGRAM,  AND  CAN  ELIMINATE 
RUNNING  THE  EXEC  FILE. 


FI  4 
FI  8 
FI  11 
FI  13 
FI  14 
FI  16 
FI  17 
FI  18 
FI  19 
FI  20 


DISK  THESIS  INPUT  (PERM 

DISK  UTILITY  DATA  (RECFM  VS  BLOCK  133  PERM 

(RECFM  F  BLOCK  80  LRECL  80  PERM 
(RECFM  VS  BLOCK  133  PERM 
(RECFM  F  BLOCK  80  LRECL  80  PERM 


DISK  CNTRL  OUTPUT 
DISK  GAMMA  OUTPUT 
DISK  MODE  OUTPUT 
DISK  COST  OUTPUT  (RECFM 
DISK  PRT  OUTPUT  (RECFM 
DISK  ERROR  DATA  (RECFM 
DISK  END  FILE   (RECFM  F 


F  BLOCK  80  LRECL  80  PERM 
F  BLOCK  80  LRECL  80  PERM 
F  BLOCK  80  LRECL  80  PERM 
BLOCK  80  LRECL  80  PERM 


DISK  GMAT  FILE  (RECFM  F  BLOCK  80  LRECL  80  PERM 


TWr^V*}ViVyriV**iV*}V*}ViVVf*iV}WriVynV****>V}V***^ 


SPA00500 
SPA00510 
SPA00520 
SPA00530 
SPA00540 
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SPA00560 
SPA00570 
SPA00580 
SPA00590 
SPA00600 
SPA00610 
SPA00620 
SPA00630 
SPA00640 
SPA00650 
SPA00660 
SPA00670 
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SPA00690 
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SPA00710 
SPA00720 
SPA00730 
SPA00740 
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SPA00800 
SPA00810 
SPA00820 
SPA00830 
SPA00840 
SPA00850 
SPA00860 
SPA00870 
SPA00880 
SPA00890 
SPA00900 
SPA00910 
SPA00920 
SPA00930 
SPA00940 
SPA00950 
SPA00960 
SPA00970 
SPA00980 
SPA00990 
SPA01000 
SPA01010 
SPA01020 
SPA01030 
SPA01040 
SPA01050 
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5 

C 

1001 

1002 

1008 

C 

500 

C 

C 

C 


700 

C 

C 

C 

C 

C 

C 

701 
C 
C 
C 

C 

702 


PARAMETER  (JR=5243,  KR=5397,  MR=262139) 


MIN  =  15.0 

WT=1.  0D00 

PI  =  4.  ODO  *  ATAN(l.ODO) 

ir/ticicMti 

*****  READ  LAMA  AND  UGVEX  MATRICIES         ***** 

*********************************************************** 

CALL  EXCMS  ('CLRSCRN') 

WRITE(6,1008) 

WRITE(6,*)  ' 

WRITE(6,*)  '  ' 

THIS  SECTION  READS  THE  LAMA  VECTOR  AND  THE  UGVEX 

MATRIX  AND  STORES  THEM  IN  MEMORY  FOR  FURTHER  RECALL  OF 

DESIRED  LOCATION  DATA. 


READING  LAMA  AND  UGVEX  MATRICIES* 


READ(4,1001)  NAM 
READ(4,1002)(LAMA(I),I=1,100) 
READ(4,1001)  NAM 
DO  5  J  =  1,100 

READ(4,1002)(UGVEX(I,J),I=1,684) 
CONTINUE 

F0RMAT(1X,A6) 
F0RMAT(1X,8E15. 8) 
F0RMAT(1X,////) 

CALL  EXCMS  (' CLRSCRN') 

************       STARTING  MODE  NUMBER 
**  SMODE  7  TO  100  (INTEGER)  **** 
SMODE=  17 


WRITE  (16,700)  SMODE 


FORMAT  ( 


STARTING  MODE  NUMBER: 


',12) 


************* 


***************    NUMBER  OF  MODES  TO  SCAN 
**  MODE  1  TO  93  (INTEGER) 

MODE=3 

EMODE  =  SMODE  +  MODE  -  1 


WRITE  (16,701)  MODE 

FORMAT  ('  ','NUMBER  OF  MODES  SCANNED:   ',12) 

************    NOISE  INPUT  POSITION      ***************** 
**  NODE  1  TO  114  (INTEGER)  (IF  0  THEN  NO  NOISE  INPUT) 
NODE=  8 


WRITE  (16,702)  NODE 


FORMAT  ( ' 


NOISE  NODE  LOCATION: 


MS) 


SPA01060 
SPA01070 
SPA01080 
SPA01090 
SPA01100 
SPA01110 
SPA01120 
SPA01130 
SPA01140 
SPA01150 
SPA01160 
SPA01170 
SPA01180 
SPA01190 
SPA01200 
SPA01210 
SPA01220 
SPA01230 
SPA01240 
SPA01250 
SPA01260 
SPA01270 
SPA01280 
SPA01290 
SPA01300 
SPA01310 
SPA01320 
SPA01330 
SPA01340 
SPA01350 
SPA01360 
SPA01370 
SPA01380 
SPA01390 
SPA01400 
SPA01410 
SPA01420 
SPA01430 
SPA01440 
SPA01450 
SPA01460 
SPA01470 
SPA01480 
SPA01490 
SPA01500 
SPA01510 
SPA01520 
SPA01530 
SPA01540 
SPA01550 
SPA01560 
SPA01570 
SPA01580 
SPA01590 
SPA01600 
SPA01610 
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899 

C 

C 


900 
C 

703 
C 
C 
C 


704 
C 

C 
C 
C 


C 
C 

711 

712 

713 

714 


***********  START  AND  STOP  FOR  PLANT 

SN=17 

FN=4 

NS=SN*2-1 

NF=SN*2+2*FN-2 

WRITE  (16,899)  SN,FN 

FORMAT  ('  ',' PLANT  --  SN=  ',15,'   FN=  ',15) 

*************         SAMPLING  TIME         ************** 

**  SAMPT  MUST  BE  LESS  THAN  OR  EQUAL  TO  SAMPTM  ** 

SAMPT  =  0.  05 

SAMPTM  =  ((2. 0D0*PI)/SQRT(LAMA(EM0DE)))/1.0D01 

IF  (SAMPT. GE.  SAMPTM)  THEN 
SAMPT=SAMPTM 

ENDIF 

WRITE  (16,900)  MIN 

FORMAT  ('  ' ,2X,'MIN:   ' ,F8. 3) 

WRITE  (16,703)  SAMPT,  SAMPTM 

FORMAT  ('  ',' SAMPLING  TIME:   ' ,D12. 4,2X, ' SAMPTM=  * ,D15.  8) 


***************        DAMPING  FACTOR 
**  DAMP  0.0  TO  1.0  (REAL*8) 
DAMP=.  01 

WRITE  (16,704)  DAMP 

FORMAT  ( '  '  , 'DAMPING  FACTOR:   ' ,D12. 4) 

N0=3 

***  PLANT  NOISE  VARIANCE  *** 

**  PNVARX,  PNVARY,  PNVARZ  GT  0. 0 

SF=1.  0D0 
SF1=2.5D06 
PNVARX=1.  0D00*SF1 
PNVARY=1. 0D00*SF1 
PNVARZ=1. 0D00*SF1 


***  MEASUREMENT  NOISE  VARIANCE  *** 
**  MNVARX,  MNVARY,  MNVARZ  GT  0.  0 
MNVARX=1. 0D-03  *SF 
MNVARY=1. OD-03  *SF 
MNVARZ=1. OD-03  *SF 


WRITE  (16,711) 

FORMATC  ',' PLANT  NOISE  VARIANCE:   ') 

WRITE  (16,712) 

FORMAT( '  ' , 6X , ' PNVARX ' , 13X , ' PNVARY  * , 13X , ' PNVARZ ' ) 

WRITE  (16,713)  PNVARX,  PNVARY,  PNVARZ 

FORMAT( '  ' ,2X,E15. 8,2X,E15. 8,2X,E15. 8) 

WRITE(16,714) 

FORMAT(  '    '  /MEASUREMENT  NOISE:  '  ) 


************** 


SPA01620 
SPA01630 
SPA01640 
SPA01650 
SPA01660 
SPA01670 
SPA01680 
SPA01690 
SPA01700 
SPA01710 
SPA01720 
SPA01730 
SPA01740 
SPA01750 
SPA01760 
SPA01770 
SPA01780 
SPA01790 
SPA01800 
SPA01810 
SPA01820 
SPA01830 
SPA01840 
SPA01850 
SPA01860 
SPA01870 
SPA01880 
SPA01890 
SPA01900 
SPA01910 
SPA01920 
SPA01930 
SPA01940 
SPA01950 
SPA01960 
SPA01970 
SPA01980 
SPA01990 
SPA02000 
SPA02010 
SPA02020 
SPA02030 
SPA02040 
SPA02050 
SPA02060 
SPA02070 
SPA02080 
SPA02090 
SPA02100 
SPA02110 
SPA02120 
SPA02130 
SPA02140 
SPA02150 
SPA02160 
SPA02170 
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715 

C 

510 


C 
C 
C 


WRITE(16,715) 

FORMATC  ' ,6X,'MNVARX' , 13X, 'MNVARY' , 13X, 'MNVARZ' ) 

WRITE( 16,713)  MNVARX, MNVARY, MNVARZ 


54 
C 


58 
60 
C 


CALL  EXCMS  ('CLRSCRN') 
WRITE  (6,1008) 
WRITE  (6,*)  ' 


NOISE  INPUT  LOCATION 


INITIALIZE  MATRICIES 


PROGRAM  RUNNING' 

************ 


**************** 


****Vc****** 

ROWN3  =  NODE*6 
ROWN2  =  (NODE*6) 
ROWN1  =  (N0DE*6) 
COUNT  =  0 

*********** 

DO  54  K  =  1,  200 

X(K)  =  0.  0 
CONTINUE 

DO  60  1=1,3 
DO  58  J=l,3 
RMN(I,J)=0.0 
QPN(I,J)=0.0 
CONTINUE 

CONTINUE 


RMN(1,1)=MNVARX**2.  0 
RMN(  2,2)  =MNVARY—  2 .  0 
RMN( 3 , 3)=MNVARZ**2.  0 
QPN(1,1)=PNVARX**2.0 
QPN(2,2)=PNVARY**2.  0 
QPN( 3 , 3 )=PNVARZ**2.  0 

»'-  «■'»  •.'-  -'-  •.'-  •.'-  J»  »'—  »  '  J  «.*  m  -.'-  ~'-  -'»  »*■•  —'-  .'  -  -"-  • '  -  «.'-  -.'»  -  '-  -' J  «.'.•  ..*.•  -.'-  •.*-  -*-  «.'.»  -.*.»  «.' J  »'«  ■-'-  -'-  «J-  •.'-  •.'»  •.'-  ..'.»  a>V  -  f  ■•  •J-  "J*  -■'»  «■'■•  •J*  — f »  »■  V  "J—  »'-  «■'■•  •-'"  »-'■•  »-'■•  "-'■»  »"-  ■■*»  -*-  »'-  «  '» 

*****  BEGIN  MAIN  PROGRAM  ***** 

j -  -j-  »»-  «.».  »t.  V"**  V*  V*  *V  *3'  ***  "V  ***  JL  ■*■ ^fc  *V  J-  V*  •J-  V*  **"  tff  "^  ■J-  *V  "V  -'■"  "**  *V  *VJ*V**1-*VV"V"  ^?  Vr  Jvy"y-Jc,VrJ"^*JfV-J"V*JfVf*V*'"V'*'5V,;V*V 

CALL  STMTRXC  EMODE , SMODE , S AMPT , DAMP , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
h   ROWNl,ROWN2,ROWN3,BN) 


WRITE   (6,1008) 
WRITE(6,*)  '  EXIT  STMTRX 


PRE -LOOP  KALMAN' 


WRITE  (6,*)  'COMPUTING  C  TIMES  SF  FOR  NEW  C 


WRITE  (16,1008) 
DO  11000  1=1,200 
DO  10900  J=l,NO 
C(J,I)=  C(  J,I)*SF 
10900    CONTINUE 
11000  CONTINUE 
C 


SPA02180 
SPA02190 
SPA02200 
SPA02210 
SPA02220 
SPA02230 
SPA02240 
SPA02250 
SPA02260 
SPA02270 
SPA02280 
SPA02290 
SPA02300 
SPA02310 
SPA02320 
SPA02330 
SPA02340 
SPA02350 
SPA02360 
SPA02370 
SPA02380 
SPA02390 
SPA02400 
SPA02410 
SPA02420 
SPA02430 
SPA02440 
SPA02450 
SPA02460 
SPA02470 
SPA02480 
SPA02490 
SPA02500 
SPA02510 
SPA02520 
SPA02530 
SPA02540 
SPA02550 
SPA02560 
SPA02570 
SPA02580 
SPA02590 
SPA02600 
SPA02610 
SPA02620 
SPA02630 
SPA02640 
SPA02650 
SPA02660 
SPA02670 
SPA02680 
SPA02690 
SPA02700 
SPA02710 
SPA02720 
SPA02730 
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8813 

C 

C 

C 

C 

C 

C 

C 


***  PRE -LOOP  PORTION  OF  KALMAN  FILTER 


JK=SMODE*2-2 
M2=2*M0DE 


***************************** 


M2=2*MODE 

jp=2*SMODE- 

JQ=2*EMODE 


DO  8813  1=1,3 
EX(I)=0.  0 

CONTINUE 


********************************************************** 

*****     THIS  SECTION  COMPUTES  THE  STATE  UPDATE     ***** 
********************************************** ************ 

ESS  =0.  0 
COUNT  =  0 
ENERGY  =  0.  ODO 
TIME  =  0. 0 
CGN=0.  0 
CTG=0.  0 
***** 


SETS  LOOP  FOR  THE  ITERATIONS 
*****  the  SYSTEM  FOR  THE  NUMBER  OF 
WRITE  (6,1008) 

WRITE  (6,*)  '  START  STATE  UPDATE 

LOOP  =  INT((MIN*60.0)/SAMPT) 
PRT=  (DBLE(LOOP))/30.0 
PRT1=(DBLE(LOOP))/50.  00 
CTT=0.  0 

DO  400  L  =0,  LOOP 

TIME  =  DBLE(L)*SAMPT 

IF(L.  EQ.  0)THEN 

IMPLSE  =(1.0D06*SF1)/(DSQRT(SAMPT)) 
ELSE 

IMPLSE  =  0.0D0 
ENDIF 

TO=0.  0 

*****  RANDOM  NUMBER  GENERATOR  ***** 

DO  101  1=1,6 

I SEED=MOD ( I SEED* JR+KR , MR ) 

RND1=(DBLE( ISEED)+0.  5D00)/DBLE(MR) 

ISEED=MOD( ISEED*JR+KR,MR) 

RND2=(DBLE(ISEED)+0.5D00)/DBLE(MR) 


NECESSARY  TO  OBSERVE  ***** 
MINUTES  SPECIFIED     ***** 


SPA02740 
SPA02750 
SPA02760 
SPA02770 
SPA02780 
SPA02790 
SPA02800 
SPA02810 
SPA02820 
SPA02830 
SPA02840 
SPA02850 
SPA02860 
SPA02870 
SPA02880 
SPA02890 
SPA02900 
SPA02910 
SPA02920 
SPA02930 
SPA02940 
SPA02950 
SPA02960 
SPA02970 
SPA02980 
SPA02990 
SPA03000 
SPA03010 
SPA03020 
SPA03030 
SPA03040 
SPA03050 
SPA03060 
SPA03070 
SPA03080 
SPA03090 
SPA03100 
SPA03110 
SPA03120 
SPA03130 
SPA03140 
SPA03150 
SPA03160 
SPA03170 
SPA03180 
SPA03190 
SPA03200 
SPA03210 
SPA03220 
SPA03230 
SPA03240 
SPA03250 
SPA03260 
SPA03270 
SPA03280 
SPA03290 
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101 

c 

c 
c 
c 
c 
c 
c 
c 


c 
c 


5010 

5015 

C 

C 

C 

C 


5030 

5035 

C 

C 

c 


c 

5040 

C 

C 

C 

C 

C 

c 


RNDM(I)=DSQRT(-2. 0*DLOG(RND1))*DCOS(6.  2831853D00*RND2) 
CONTINUE 

CTT=CTT+1. 0 

****  START  OF  STATE  UPDATE  *** 

***  COMPUTE  AX°200  =  A0 200  X  200  *  X°200 


***  COMPUTE  AXH  =  A*XH 

JK=SMODE*2-2 

JP=JK+1 

JQ=2*EMODE 


DO  5015  I=NS,NF 
SUM=0.  0 

DO  5010  K=NS,NF 
SUM=SUM+A(I,K)*X(K) 
CONTINUE 
AX(I)=SUM 
CONTINUE 


***  COMPUTE  WD°3 

WD( 1)=PNVARX*RNDM( l)*TO+IMPLSE 
WD ( 2 ) =PNVARY*RNDM( 2 ) *TO 
WD( 3 )=PNVARZ*RNDM( 3 ) -TO 

***  COMPUTE  BNVD°200  =BN°200  X  3 

DO  5035  I=NS,NF 
SUM=0. 0 

DO  5030  K=l,3 
SUM=SUM+BN( I ,K)*WD(K) 
CONTINUE 
BNWD(I)=SUM 
CONTINUE 

***  COMPUTE  X°200  =AX°200  +  BNWD°200 

DO  5040  I=NS,NF 

X(I)=  AX(I)  +  BNWD(I) 

IF  (DABS(X(I)).LT.  1.  OD-60)  THEN 

X(I)=1.  OD-60 
END  IF 

CONTINUE 


WD°3 


***  START  OF  KALMAN  FILTER  *** 


SPA03300 
SPA03310 
SPA03320 
SPA03330 
SPA03340 
SPA03350 
SPA03360 
SPA03370 
SPA03380 
SPA03390 
SPA03400 
SPA03410 
SPA03420 
SPA03430 
SPA03440 
SPA03450 
SPA03460 
SPA03470 
SPA03480 
SPA03490 
SPA03500 
SPA03510 
SPA03520 
SPA03530 
SPA03540 
SPA03550 
SPA03560 
SPA03570 
SPA03580 
SPA03590 
SPA03600 
SPA03610 
SPA03620 
SPA03630 
SPA03640 
SPA03650 
SPA03660 
SPA03670 
SPA03680 
SPA03690 
SPA03700 
SPA03710 
SPA03720 
SPA03730 
SPA03740 
SPA03750 
SPA03760 
SPA03770 
SPA03780 
SPA03790 
SPA03800 
SPA03810 
SPA03820 
SPA03830 
SPA03840 
SPA03850 
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8887 

8888 

C 

C 


380 

4500 

C 


C 
C 

400 


9499 

C 

C 

C 

C 

C 


C 
599 

C 
C 
C 
C 
C 


JK=SMODE*2-2 
JP=JK+1 
JQ=2*EMODE 
M2=2*MODE 

JL=JQ+1 

DO  8888  1=1, NO 

SUM=0.  0 

DO  8887  K=JL,NF 

SUM=SUM+C(I,K)*X(K) 

CONTINUE 

EX( I )=SUM*SUM*SAMPT+EX( I ) 

CONTINUE 


CGN=CGN+1.  0 

IF  (CTT.EQ.  1.  O.OR.  CGN.  GT.  PRT)  THEN 

WRITE  (16,1008) 

WRITE  (16,*)    'TIME  =  ',  TIME 

DO  380  I=JP  ,  JQ 

WRITE  (16,4500)  I,X(I) 

CONTINUE 

FORMAT  ('  * ,2X,'X(' ,14,')=  ' ,D15.8) 

CGN=0. 0 
END  IF 


CONTINUE 

WRITE  (11,*)  'SMODE  =  ',  SMODE 

WRITE  (11,*)  'EMODE  =  ',  EMODE 

WRITE  (11,*)  'SN  =  ' ,SN 

WRITE  (11,*)  'FN  =  ' ,FN 


JL=JQ+1 

DO  9499  1=1, NO 

WRITE  (11,*)  'EX   ' ,1 

CONTINUE 


EX(I) 


CALL  EXCMS  ('CLRSCRN') 
WRITE  (6,1008) 

STOP 

END 


SPA03860 
SPA03870 
SPA03880 
SPA03890 
SPA03900 
SPA03910 
SPA03920 
SPA03930 
SPA03940 
SPA03950 
SPA03960 
SPA03970 
SPA03980 
SPA03990 
SPA04000 
SPA04010 
SPA04020 
SPA04030 
SPA0404.0 
SPA04050 
SPA04060 
SPA04070 
SPA04080 
SPA04090 
SPA04100 
SPA04110 
SPA04120 
SPA04130 
SPA04140 
SPA04150 
SPA04160 
SPA04170 
SPA04180 
SPA04190 
SPA04200 
SPA04210 
SPA04220 
SPA04230 
SPA04240 
SPA04250 
SPA04260 
SPA04270 
SPA04280 
SPA04290 
SPA04300 
SPA04310 
SPA04320 
SPA04330 
SPA04340 
SPA04350 
SPA04360 
SPA04370 
SPA04380 
SPA04390 
SPA04400 
SPA04410 
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c 
c 

c 
c 


THIS  SUBROUTINE  COMPUTES  THE  STATE  TRANSITION  MATRIX  FOR  EACH 
OF  THE  100  MODES 

SUBROUTINE  STMTRX( EMODE , SMODE , T , D , PHI , GAMMA , A , B , LAMA , UGVEX , C , 
+        R0WN1,R0WN2,R0WN3,BN) 

REAL*8  WN,GMA,PHI(2,2,100),GAMMA(2,100),EGT,T,COSW1T,SINW1T 

REAL*8  W1,D,A(200,200),B(200,3),C(9J200),BN(200,3) 

REAL  LAMA(100),UGVEX(684,100) 

INTEGER  SMODE, R, EMODE, JJ,KK,ROWNl,ROWN2,ROWN3,  NN(9),  N9 ,  NO 


WRITE  (6,*)  'INSIDE  STMTRX  -  -  COMPUTE  WN,  GMA,  EFT,  Wl' 


DO  600  I  =  1     ,100 

WN  =  DBLE(SQRT(LAMA(I))) 

GMA  =  D-WN/2. 0 

EGT  =  DEXP(-GMA*T) 

Wl  =  DSQRT((WN**2)-(GMA**2)) 

COSW1T  =  DC0S(W1*T) 

SINW1T  =  DSIN(Wl--T) 


IF(WN.  EQ.  0)THEN 

PHI(1, 

1,1) 

= 

EGT*C0SW1T 

PHI(13 

2,D 

= 

T 

PHI(2, 

1,D 

= 

0 

PHI(2, 

2,1) 

= 

EGT*C0SW1T 

ELSE 


GAMMA(1,I)  =  0 
GAMMA(2,I)  =  0 


PHI(1,1,I)  =  EGT*(C0SW1T  +  (GMA*(W1**(-1)))*SINW1T) 

PHI( 1,2,1)  =  (W1**(-1))*EGT*SINW1T 

PHI(2,1,I)  =  -(WN**2)*(W1**(-1))*EGT*SINW1T 

PHI (2, 2, I)  =  EGT*(C0SW1T  -  (GMA*(W1**(-1)))*SINW1T) 


GAMMA(1,I)=(WN**(-2))*(1.0DO-EGT*COSW1T  -EGT*(GMA/W1)*SINW1T) 


SPA04420 
SPA04430 
SPA04440 
SPA04450 
SPA04460 
SPA04470 
SPA04480 
SPA04490 
SPA04500 
SPA04510 
SPA04520 
SPA04530 
SPA04540 
SPA04550 
SPA04560 
SPA04570 
SPA04580 
SPA04590 
SPA04600 
SPA04610 
SPA04620 
SPA04630 
SPA04640 
SPA04650 
SPA04660 
SPA04670 
SPA04680 
SPA04690 
SPA04700 
SPA04710 
SPA04720 
SPA04730 
SPA04740 
SPA04750 
SPA04760 
SPA04770 
SPA04780 
SPA04790 
SPA04800 
SPA04810 
SPA04820 
SPA04830 
SPA04840 
SPA04850 
SPA04860 
SPA04870 
SPA04880 
SPA04890 
SPA04900 
SPA04910 
SPA04920 
SPA04930 
SPA04940 
SPA04950 
SPA04960 
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c 
c 

c 
c 

c 
c 
c 

600 

c 


GAMMA(2,I)  =  (W1**(-1))*EGT*SINW1T 


ENDIF 


CONTINUE 

WRITE  (6,*)  'PHI  AND  GAMMA  COMPUTED' 
WRITE  (6,*)  '  COMPUTING  A,  B,  BN' 


R  =  1 

DO  610  K  =  1 


,100 


A(R,R)  =  PHI(1,1,K) 
A(R,R+1)  =  PHI(1,2,K) 
A(R+1,R)  =  PHI(2,1,K) 
A(R+1,R+1)  =  PHI(2,2,K) 


•  B  MATRIX  FOR  MULTIPLYING  CONTROL  TORQUES 

B(R,1)  =  GAMMA(1,K)*DBLE(UGVEX(412,K)) 
B(R,2)  =  GAMMA(1,K)*DBLE(UGVEX(413,K)) 
B(R,3)  =  GAMMA(1,K)*DBLE(UGVEX(414,K)) 
B(R+1,1)  =  GAMMA(2,K)*DBLE(UGVEX(412,K)) 
B(R+1,2)  =  GAMMA(2,K)*DBLE(UGVEX(413,K)) 
B(R+1,3)  =  GAMMA(2,K)*DBLE(UGVEX(414,K)) 


***  BN  MATRIX  FOR  MULTIPLYING  THE  NOISE  DISTURBANCES 


BN(R,1)=GAMMA(1,K)*DBLE(UGVEX(R0WN1,K)) 

BN(R,2)=GAMMA(l,K)*DBLE(UGVEX(ROWN2,K)) 

BN(R,3)=GAMMA(1,K)*DBLE(UGVEX(R0WN3,K)) 

BN(R+1,1)=GAMMA(2,K)*DBLE(UGVEX(R0WN1,K)) 

BN(R+1,2)=GAMMA(2,K)*DBLE(UGVEX(R0WN2,K)) 


SPA04970 
SPA04980 
SPA04990 
SPA05000 
SPA05010 
SPA05020 
SPA05030 
SPA05040 
SPA05050 
SPA05060 
SPA05070 
SPA05080 
SPA05090 
SPA05100 
SPA05110 
SPA05120 
SPA05130 
SPA05140 
SPA05150 
SPA05160 
SPA05170 
SPA05180 
SPA05190 
SPA05200 
SPA05210 
SPA05220 
SPA05230 
SPA05240 
SPA05250 
SPA05260 
SPA05270 
SPA05280 
SPA05290 
SPA05300 
SPA05310 
SPA05320 
SPA05330 
SPA05340 
SPA05350 
SPA05360 
SPA05370 
SPA05380 
SPA05390 
SPA05400 
SPA05410 
SPA05420 
SPA05430 
SPA05440 
SPA05450 
SPA05460 
SPA05470 
SPA05480 
SPA05490 
SPA05500 
SPA05510 
SPA05520 
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c 

G 
C 
C 
C 
C 

610 
C 


C 

C 

C 

C 

C 

C 

9127 

640 

C 

C 

C 


BN(R+l,3)=GAMMA(2,K)*DBLE(UGVEX(ROWN3,K)) 


R  =  R+2 

CONTINUE 

WRITE  (6,*)  'a,  B,  BN  COMPUTED' 
WRITE  (6,*)  'COMPUTING  C* 


***  c  MATRIX  PRODUCTION  *** 

NO=3 

NN(1)=418 

NN(2)=419 

NN(3)=420 


JJ=-1 

DO  640  1=1,100 

JJ=JJ+1 

DO  9127  K=l,NO 

KK=I+JJ 

N9=NN(K) 

C(K,KK)  =  DBLE(UGVEX(N9,I)) 

KK=KK+1 

C(K,KK)=0.  0 

CONTINUE 
CONTINUE 


RETURN 
END 


SPA05530 
SPA05540 
SPA05550 
SPA05560 
SPA05570 
SPA05580 
SPA05590 
SPA05600 
SPA05610 
SPA05620 
SPA05630 
SPA05640 
SPA05650 
SPA05660 
SPA05670 
SPA05680 
SPA05690 
SPA05700 
SPA05710 
SPA05720 
SPA05730 
SPA05  740 
SPA05750 
SPA05760 
SPA05770 
SPA05780 
SPA05790 
SPA05800 
SPA05810 
SPA05820 
SPA05830 
SPA05840 
SPA05850 
SPA05860 
SPA05870 
SPA05880 
SPA05890 
SPA05900 
SPA05910 
SPA05920 
SPA05930 
SPA05940 
SPA05950 
SPA05960 
SPA05970 
SPA05980 
SPA05990 
SPA06000 
SPA06010 
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